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,
];

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_lambda2: [usize; 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 {
@@ -116,7 +116,7 @@ impl WVelocityConstraint {
mj_lambda1,
mj_lambda2,
manifold_id,
manifold_contact_id: l,
manifold_contact_id: [[0; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
num_contacts: num_points as u8,
};
@@ -141,6 +141,8 @@ impl WVelocityConstraint {
let vel2 = linvel2 + angvel2.gcross(dp2);
constraint.limit = friction;
constraint.manifold_contact_id[k] =
array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH];
// Normal part.
{
@@ -332,17 +334,17 @@ impl WVelocityConstraint {
for ii in 0..SIMD_WIDTH {
let manifold = &mut manifolds_all[self.manifold_id[ii]];
let k_base = self.manifold_contact_id;
let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()];
active_contacts[k_base + k].data.impulse = impulses[ii];
let contact_id = self.manifold_contact_id[k][ii];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.impulse = impulses[ii];
#[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")]
{
active_contacts[k_base + k].data.tangent_impulse =
active_contact.data.tangent_impulse =
[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 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: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS],
}
@@ -92,7 +92,7 @@ impl VelocityGroundConstraint {
limit: 0.0,
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,
};
@@ -133,7 +133,7 @@ impl VelocityGroundConstraint {
constraint.limit = 0.0;
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;
}
@@ -145,6 +145,7 @@ impl VelocityGroundConstraint {
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
// Normal part.
{
@@ -267,19 +268,18 @@ impl VelocityGroundConstraint {
// 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]) {
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,
];

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

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