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_lambda1: usize,
pub mj_lambda2: usize, pub mj_lambda2: usize,
pub manifold_id: ContactManifoldIndex, pub manifold_id: ContactManifoldIndex,
pub manifold_contact_id: usize, pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
pub num_contacts: u8, pub num_contacts: u8,
pub elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS], pub elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS],
} }
@@ -168,7 +168,7 @@ impl VelocityConstraint {
mj_lambda1, mj_lambda1,
mj_lambda2, mj_lambda2,
manifold_id, manifold_id,
manifold_contact_id: l * MAX_MANIFOLD_POINTS, manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
num_contacts: manifold_points.len() as u8, num_contacts: manifold_points.len() as u8,
}; };
@@ -211,7 +211,7 @@ impl VelocityConstraint {
constraint.mj_lambda1 = mj_lambda1; constraint.mj_lambda1 = mj_lambda1;
constraint.mj_lambda2 = mj_lambda2; constraint.mj_lambda2 = mj_lambda2;
constraint.manifold_id = manifold_id; 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; constraint.num_contacts = manifold_points.len() as u8;
} }
@@ -224,6 +224,8 @@ impl VelocityConstraint {
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction; constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
// Normal part. // Normal part.
{ {
let gcross1 = rb1 let gcross1 = rb1
@@ -382,19 +384,18 @@ impl VelocityConstraint {
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
let manifold = &mut manifolds_all[self.manifold_id]; let manifold = &mut manifolds_all[self.manifold_id];
let k_base = self.manifold_contact_id;
for k in 0..self.num_contacts as usize { for k in 0..self.num_contacts as usize {
let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()]; let contact_id = self.manifold_contact_id[k];
active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse; let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.impulse = self.elements[k].normal_part.impulse;
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
{ {
active_contacts[k_base + k].data.tangent_impulse = active_contacts.data.tangent_impulse = self.elements[k].tangent_part[0].impulse;
self.elements[k].tangent_part[0].impulse;
} }
#[cfg(feature = "dim3")] #[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[0].impulse,
self.elements[k].tangent_part[1].impulse, self.elements[k].tangent_part[1].impulse,
]; ];

16
src/dynamics/solver/velocity_constraint_wide.rs
View File

@@ -55,7 +55,7 @@ pub(crate) struct WVelocityConstraint {
pub mj_lambda1: [usize; SIMD_WIDTH], pub mj_lambda1: [usize; SIMD_WIDTH],
pub mj_lambda2: [usize; SIMD_WIDTH], pub mj_lambda2: [usize; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
pub manifold_contact_id: usize, pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
} }
impl WVelocityConstraint { impl WVelocityConstraint {
@@ -116,7 +116,7 @@ impl WVelocityConstraint {
mj_lambda1, mj_lambda1,
mj_lambda2, mj_lambda2,
manifold_id, manifold_id,
manifold_contact_id: l, manifold_contact_id: [[0; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
num_contacts: num_points as u8, num_contacts: num_points as u8,
}; };
@@ -141,6 +141,8 @@ impl WVelocityConstraint {
let vel2 = linvel2 + angvel2.gcross(dp2); let vel2 = linvel2 + angvel2.gcross(dp2);
constraint.limit = friction; constraint.limit = friction;
constraint.manifold_contact_id[k] =
array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH];
// Normal part. // Normal part.
{ {
@@ -332,17 +334,17 @@ impl WVelocityConstraint {
for ii in 0..SIMD_WIDTH { for ii in 0..SIMD_WIDTH {
let manifold = &mut manifolds_all[self.manifold_id[ii]]; let manifold = &mut manifolds_all[self.manifold_id[ii]];
let k_base = self.manifold_contact_id; let contact_id = self.manifold_contact_id[k][ii];
let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()]; let active_contact = &mut manifold.points[contact_id as usize];
active_contacts[k_base + k].data.impulse = impulses[ii]; active_contact.data.impulse = impulses[ii];
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
{ {
active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii]; active_contact.data.tangent_impulse = tangent_impulses[ii];
} }
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
{ {
active_contacts[k_base + k].data.tangent_impulse = active_contact.data.tangent_impulse =
[tangent_impulses[ii], bitangent_impulses[ii]]; [tangent_impulses[ii], bitangent_impulses[ii]];
} }
} }

18
src/dynamics/solver/velocity_ground_constraint.rs
View File

@@ -49,7 +49,7 @@ pub(crate) struct VelocityGroundConstraint {
pub limit: Real, pub limit: Real,
pub mj_lambda2: usize, pub mj_lambda2: usize,
pub manifold_id: ContactManifoldIndex, pub manifold_id: ContactManifoldIndex,
pub manifold_contact_id: usize, pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
pub num_contacts: u8, pub num_contacts: u8,
pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS],
} }
@@ -92,7 +92,7 @@ impl VelocityGroundConstraint {
limit: 0.0, limit: 0.0,
mj_lambda2, mj_lambda2,
manifold_id, manifold_id,
manifold_contact_id: l * MAX_MANIFOLD_POINTS, manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
num_contacts: manifold_points.len() as u8, num_contacts: manifold_points.len() as u8,
}; };
@@ -133,7 +133,7 @@ impl VelocityGroundConstraint {
constraint.limit = 0.0; constraint.limit = 0.0;
constraint.mj_lambda2 = mj_lambda2; constraint.mj_lambda2 = mj_lambda2;
constraint.manifold_id = manifold_id; 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; constraint.num_contacts = manifold_points.len() as u8;
} }
@@ -145,6 +145,7 @@ impl VelocityGroundConstraint {
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction; constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
// Normal part. // Normal part.
{ {
@@ -267,19 +268,18 @@ impl VelocityGroundConstraint {
// FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint. // FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint.
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
let manifold = &mut manifolds_all[self.manifold_id]; let manifold = &mut manifolds_all[self.manifold_id];
let k_base = self.manifold_contact_id;
for k in 0..self.num_contacts as usize { for k in 0..self.num_contacts as usize {
let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()]; let contact_id = self.manifold_contact_id[k];
active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse; let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.impulse = self.elements[k].normal_part.impulse;
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
{ {
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[0].impulse;
} }
#[cfg(feature = "dim3")] #[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[0].impulse,
self.elements[k].tangent_part[1].impulse, self.elements[k].tangent_part[1].impulse,
]; ];

16
src/dynamics/solver/velocity_ground_constraint_wide.rs
View File

@@ -51,7 +51,7 @@ pub(crate) struct WVelocityGroundConstraint {
pub limit: SimdReal, pub limit: SimdReal,
pub mj_lambda2: [usize; SIMD_WIDTH], pub mj_lambda2: [usize; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
pub manifold_contact_id: usize, pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
} }
impl WVelocityGroundConstraint { impl WVelocityGroundConstraint {
@@ -111,7 +111,7 @@ impl WVelocityGroundConstraint {
limit: SimdReal::splat(0.0), limit: SimdReal::splat(0.0),
mj_lambda2, mj_lambda2,
manifold_id, manifold_id,
manifold_contact_id: l, manifold_contact_id: [[0; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
num_contacts: num_points as u8, num_contacts: num_points as u8,
}; };
@@ -135,6 +135,8 @@ impl WVelocityGroundConstraint {
let vel2 = linvel2 + angvel2.gcross(dp2); let vel2 = linvel2 + angvel2.gcross(dp2);
constraint.limit = friction; constraint.limit = friction;
constraint.manifold_contact_id[k] =
array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH];
// Normal part. // Normal part.
{ {
@@ -281,17 +283,17 @@ impl WVelocityGroundConstraint {
for ii in 0..SIMD_WIDTH { for ii in 0..SIMD_WIDTH {
let manifold = &mut manifolds_all[self.manifold_id[ii]]; let manifold = &mut manifolds_all[self.manifold_id[ii]];
let k_base = self.manifold_contact_id; let contact_id = self.manifold_contact_id[k][ii];
let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()]; let active_contact = &mut manifold.points[contact_id as usize];
active_contacts[k_base + k].data.impulse = impulses[ii]; active_contact.data.impulse = impulses[ii];
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
{ {
active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii]; active_contact.data.tangent_impulse = tangent_impulses[ii];
} }
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
{ {
active_contacts[k_base + k].data.tangent_impulse = active_contact.data.tangent_impulse =
[tangent_impulses[ii], bitangent_impulses[ii]]; [tangent_impulses[ii], bitangent_impulses[ii]];
} }
} }

8
src/geometry/contact_pair.rs
View File

@@ -110,6 +110,8 @@ pub struct ContactManifoldData {
#[derive(Copy, Clone, Debug)] #[derive(Copy, Clone, Debug)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct SolverContact { pub struct SolverContact {
/// The index of the manifold contact used to generate this solver contact.
pub contact_id: u8,
/// The world-space contact point. /// The world-space contact point.
pub point: Point<Real>, pub point: Point<Real>,
/// The distance between the two original contacts points along the contact normal. /// The distance between the two original contacts points along the contact normal.
@@ -203,3 +205,9 @@ impl ContactManifoldData {
// manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier() // manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier()
// } // }
} }
/// A contact manifold that can be modified by the user.
pub struct ModifiableContactManifold<'a> {
manifold: &'a super::ContactManifold,
solver_contacts: &'a mut Vec<SolverContact>,
}

20
src/geometry/narrow_phase.rs
View File

@@ -546,15 +546,17 @@ impl NarrowPhase {
manifold.data.solver_flags = solver_flags; manifold.data.solver_flags = solver_flags;
manifold.data.normal = world_pos1 * manifold.local_n1; manifold.data.normal = world_pos1 * manifold.local_n1;
// Sort contacts to keep only these with distances bellow // Generate solver contacts.
// the prediction, and generate solver contacts. for (contact_id, contact) in manifold.points.iter().enumerate() {
let mut first_inactive_index = manifold.points.len(); assert!(
contact_id <= u8::MAX as usize,
"A contact manifold cannot contain more than 255 contacts currently."
);
while manifold.data.num_active_contacts() != first_inactive_index {
let contact = &manifold.points[manifold.data.num_active_contacts()];
if contact.dist < prediction_distance { if contact.dist < prediction_distance {
// Generate the solver contact. // Generate the solver contact.
let solver_contact = SolverContact { let solver_contact = SolverContact {
contact_id: contact_id as u8,
point: world_pos1 * contact.local_p1 point: world_pos1 * contact.local_p1
+ manifold.data.normal * contact.dist / 2.0, + manifold.data.normal * contact.dist / 2.0,
dist: contact.dist, dist: contact.dist,
@@ -570,14 +572,6 @@ impl NarrowPhase {
has_any_active_contact = true; has_any_active_contact = true;
continue; continue;
} }
// If we reach this code, then the contact must be ignored by the constraints solver.
// Swap with the last contact.
manifold.points.swap(
manifold.data.num_active_contacts(),
first_inactive_index - 1,
);
first_inactive_index -= 1;
} }
} }