aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/dynamics/solver/velocity_constraint.rs19
-rw-r--r--src/dynamics/solver/velocity_constraint_wide.rs16
-rw-r--r--src/dynamics/solver/velocity_ground_constraint.rs18
-rw-r--r--src/dynamics/solver/velocity_ground_constraint_wide.rs16
-rw-r--r--src/geometry/contact_pair.rs8
-rw-r--r--src/geometry/narrow_phase.rs20
6 files changed, 52 insertions, 45 deletions
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 2cac93d..63139cc 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],
}
@@ -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,
];
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index 3e068e7..8838011 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -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]];
}
}
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index 88f864a..6f61b17 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -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,
];
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index 29ba5e9..6f279d8 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -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]];
}
}
diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs
index 462d3ef..c94a2cf 100644
--- a/src/geometry/contact_pair.rs
+++ b/src/geometry/contact_pair.rs
@@ -110,6 +110,8 @@ pub struct ContactManifoldData {
#[derive(Copy, Clone, Debug)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct SolverContact {
+ /// The index of the manifold contact used to generate this solver contact.
+ pub contact_id: u8,
/// The world-space contact point.
pub point: Point<Real>,
/// 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()
// }
}
+
+/// A contact manifold that can be modified by the user.
+pub struct ModifiableContactManifold<'a> {
+ manifold: &'a super::ContactManifold,
+ solver_contacts: &'a mut Vec<SolverContact>,
+}
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 640ce12..9513fef 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -546,15 +546,17 @@ impl NarrowPhase {
manifold.data.solver_flags = solver_flags;
manifold.data.normal = world_pos1 * manifold.local_n1;
- // Sort contacts to keep only these with distances bellow
- // the prediction, and generate solver contacts.
- let mut first_inactive_index = manifold.points.len();
+ // Generate solver contacts.
+ for (contact_id, contact) in manifold.points.iter().enumerate() {
+ 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 {
// Generate the solver contact.
let solver_contact = SolverContact {
+ contact_id: contact_id as u8,
point: world_pos1 * contact.local_p1
+ manifold.data.normal * contact.dist / 2.0,
dist: contact.dist,
@@ -570,14 +572,6 @@ impl NarrowPhase {
has_any_active_contact = true;
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;
}
}
generated by cgit (git 2.34.1) at 2026-02-03 21:56:34 +0000