Allow removing a rigid-body without auto-removing attached colliders

This commit is contained in:
Sébastien Crozet
2022-02-20 14:21:59 +01:00
committed by Sébastien Crozet
parent 412fedf7e3
commit 28cc19d104
7 changed files with 327 additions and 236 deletions

5
.vscode/tasks.json vendored
View File

@@ -2,6 +2,11 @@
// See https://go.microsoft.com/fwlink/?LinkId=733558 // See https://go.microsoft.com/fwlink/?LinkId=733558
// for the documentation about the tasks.json format // for the documentation about the tasks.json format
"version": "2.0.0", "version": "2.0.0",
"options": {
"env": {
"RUST_BACKTRACE": "1"
}
},
"tasks": [ "tasks": [
{ {
"label": "run 3d (no-simd - release) ", "label": "run 3d (no-simd - release) ",

View File

@@ -63,6 +63,7 @@ pub fn init_world(testbed: &mut Testbed) {
&mut physics.colliders, &mut physics.colliders,
&mut physics.impulse_joints, &mut physics.impulse_joints,
&mut physics.multibody_joints, &mut physics.multibody_joints,
true,
); );
if let Some(graphics) = &mut graphics { if let Some(graphics) = &mut graphics {

View File

@@ -141,6 +141,7 @@ impl RigidBodySet {
colliders: &mut ColliderSet, colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet, impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet, multibody_joints: &mut MultibodyJointSet,
remove_attached_colliders: bool,
) -> Option<RigidBody> { ) -> Option<RigidBody> {
let rb = self.bodies.remove(handle.0)?; let rb = self.bodies.remove(handle.0)?;
/* /*
@@ -151,8 +152,16 @@ impl RigidBodySet {
/* /*
* Remove colliders attached to this rigid-body. * Remove colliders attached to this rigid-body.
*/ */
for collider in rb.colliders() { if remove_attached_colliders {
colliders.remove(*collider, islands, self, false); for collider in rb.colliders() {
colliders.remove(*collider, islands, self, false);
}
} else {
// If we dont remove the attached colliders, simply detach them.
let colliders_to_detach = rb.colliders().to_vec();
for co_handle in colliders_to_detach {
colliders.set_parent(co_handle, None, self);
}
} }
/* /*

View File

@@ -6,6 +6,7 @@ use crate::geometry::{
ColliderParent, ColliderPosition, ColliderShape, ColliderType, ColliderParent, ColliderPosition, ColliderShape, ColliderType,
}; };
use crate::geometry::{ColliderChanges, ColliderHandle}; use crate::geometry::{ColliderChanges, ColliderHandle};
use crate::math::Isometry;
use std::ops::{Index, IndexMut}; use std::ops::{Index, IndexMut};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -180,6 +181,54 @@ impl ColliderSet {
handle handle
} }
/// Sets the parent of the given collider.
// TODO: find a way to define this as a method of Collider.
pub fn set_parent(
&mut self,
handle: ColliderHandle,
new_parent_handle: Option<RigidBodyHandle>,
bodies: &mut RigidBodySet,
) {
if let Some(collider) = self.get_mut(handle) {
let curr_parent = collider.co_parent.map(|p| p.handle);
if new_parent_handle == curr_parent {
return; // Nothing to do, this is the same parent.
}
collider.co_changes |= ColliderChanges::PARENT;
if let Some(parent_handle) = curr_parent {
if let Some(rb) = bodies.get_mut(parent_handle) {
rb.remove_collider_internal(handle, &*collider);
}
}
match new_parent_handle {
Some(new_parent_handle) => {
if let Some(co_parent) = &mut collider.co_parent {
co_parent.handle = new_parent_handle;
} else {
collider.co_parent = Some(ColliderParent {
handle: new_parent_handle,
pos_wrt_parent: Isometry::identity(),
})
};
if let Some(rb) = bodies.get_mut(new_parent_handle) {
rb.add_collider(
handle,
collider.co_parent.as_ref().unwrap(),
&mut collider.co_pos,
&collider.co_shape,
&collider.co_mprops,
);
}
}
None => collider.co_parent = None,
}
}
}
/// Remove a collider from this set and update its parent accordingly. /// Remove a collider from this set and update its parent accordingly.
/// ///
/// If `wake_up` is `true`, the rigid-body the removed collider is attached to /// If `wake_up` is `true`, the rigid-body the removed collider is attached to

View File

@@ -74,6 +74,12 @@ impl ContactPair {
} }
} }
pub fn clear(&mut self) {
self.manifolds.clear();
self.has_any_active_contact = false;
self.workspace = None;
}
/// Finds the contact with the smallest signed distance. /// Finds the contact with the smallest signed distance.
/// ///
/// If the colliders involved in this contact pair are penetrating, then /// If the colliders involved in this contact pair are penetrating, then

View File

@@ -385,7 +385,7 @@ impl NarrowPhase {
if let Some(co_changes) = co_changes { if let Some(co_changes) = co_changes {
if co_changes.needs_narrow_phase_update() { if co_changes.needs_narrow_phase_update() {
// No flag relevant to the narrow-phase is enabled for this collider. // No flag relevant to the narrow-phase is enabled for this collider.
return; continue;
} }
if let Some(gid) = self.graph_indices.get(handle.0) { if let Some(gid) = self.graph_indices.get(handle.0) {
@@ -712,87 +712,97 @@ impl NarrowPhase {
par_iter_mut!(&mut self.intersection_graph.graph.edges).for_each(|edge| { par_iter_mut!(&mut self.intersection_graph.graph.edges).for_each(|edge| {
let handle1 = nodes[edge.source().index()].weight; let handle1 = nodes[edge.source().index()].weight;
let handle2 = nodes[edge.target().index()].weight; let handle2 = nodes[edge.target().index()].weight;
let mut had_intersection = edge.weight;
let co_parent1: Option<&ColliderParent> = colliders.get(handle1.0); // TODO: remove the `loop` once labels on blocks is stabilized.
let (co_changes1, co_shape1, co_pos1, co_flags1): ( 'emit_events: loop {
&ColliderChanges, let co_parent1: Option<&ColliderParent> = colliders.get(handle1.0);
&ColliderShape, let (co_changes1, co_shape1, co_pos1, co_flags1): (
&ColliderPosition, &ColliderChanges,
&ColliderFlags, &ColliderShape,
) = colliders.index_bundle(handle1.0); &ColliderPosition,
&ColliderFlags,
) = colliders.index_bundle(handle1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(handle2.0); let co_parent2: Option<&ColliderParent> = colliders.get(handle2.0);
let (co_changes2, co_shape2, co_pos2, co_flags2): ( let (co_changes2, co_shape2, co_pos2, co_flags2): (
&ColliderChanges, &ColliderChanges,
&ColliderShape, &ColliderShape,
&ColliderPosition, &ColliderPosition,
&ColliderFlags, &ColliderFlags,
) = colliders.index_bundle(handle2.0); ) = colliders.index_bundle(handle2.0);
if !co_changes1.needs_narrow_phase_update() && !co_changes2.needs_narrow_phase_update() if !co_changes1.needs_narrow_phase_update()
{ && !co_changes2.needs_narrow_phase_update()
// No update needed for these colliders. {
return; // No update needed for these colliders.
}
// TODO: avoid lookup into bodies.
let mut rb_type1 = RigidBodyType::Static;
let mut rb_type2 = RigidBodyType::Static;
if let Some(co_parent1) = co_parent1 {
rb_type1 = *bodies.index(co_parent1.handle.0);
}
if let Some(co_parent2) = co_parent2 {
rb_type2 = *bodies.index(co_parent2.handle.0);
}
// Filter based on the rigid-body types.
if !co_flags1.active_collision_types.test(rb_type1, rb_type2)
&& !co_flags2.active_collision_types.test(rb_type1, rb_type2)
{
return;
}
// Filter based on collision groups.
if !co_flags1.collision_groups.test(co_flags2.collision_groups) {
return;
}
let active_hooks = co_flags1.active_hooks | co_flags2.active_hooks;
let active_events = co_flags1.active_events | co_flags2.active_events;
if active_hooks.contains(ActiveHooks::FILTER_INTERSECTION_PAIR) {
let context = PairFilterContext {
bodies,
colliders,
rigid_body1: co_parent1.map(|p| p.handle),
rigid_body2: co_parent2.map(|p| p.handle),
collider1: handle1,
collider2: handle2,
};
if !hooks.filter_intersection_pair(&context) {
// No intersection allowed.
return; return;
} }
// TODO: avoid lookup into bodies.
let mut rb_type1 = RigidBodyType::Static;
let mut rb_type2 = RigidBodyType::Static;
if let Some(co_parent1) = co_parent1 {
rb_type1 = *bodies.index(co_parent1.handle.0);
}
if let Some(co_parent2) = co_parent2 {
rb_type2 = *bodies.index(co_parent2.handle.0);
}
// Filter based on the rigid-body types.
if !co_flags1.active_collision_types.test(rb_type1, rb_type2)
&& !co_flags2.active_collision_types.test(rb_type1, rb_type2)
{
edge.weight = false;
break 'emit_events;
}
// Filter based on collision groups.
if !co_flags1.collision_groups.test(co_flags2.collision_groups) {
edge.weight = false;
break 'emit_events;
}
let active_hooks = co_flags1.active_hooks | co_flags2.active_hooks;
if active_hooks.contains(ActiveHooks::FILTER_INTERSECTION_PAIR) {
let context = PairFilterContext {
bodies,
colliders,
rigid_body1: co_parent1.map(|p| p.handle),
rigid_body2: co_parent2.map(|p| p.handle),
collider1: handle1,
collider2: handle2,
};
if !hooks.filter_intersection_pair(&context) {
// No intersection allowed.
edge.weight = false;
break 'emit_events;
}
}
let pos12 = co_pos1.inv_mul(co_pos2);
edge.weight = query_dispatcher
.intersection_test(&pos12, &**co_shape1, &**co_shape2)
.unwrap_or(false);
break 'emit_events;
} }
let pos12 = co_pos1.inv_mul(co_pos2); let co_flags1: &ColliderFlags = colliders.index(handle1.0);
let co_flags2: &ColliderFlags = colliders.index(handle2.0);
let active_events = co_flags1.active_events | co_flags2.active_events;
if let Ok(intersection) = if active_events.contains(ActiveEvents::INTERSECTION_EVENTS)
query_dispatcher.intersection_test(&pos12, &**co_shape1, &**co_shape2) && had_intersection != edge.weight
{ {
if active_events.contains(ActiveEvents::INTERSECTION_EVENTS) events.handle_intersection_event(IntersectionEvent::new(
&& intersection != edge.weight handle1,
{ handle2,
events.handle_intersection_event(IntersectionEvent::new( edge.weight,
handle1, ));
handle2,
intersection,
));
}
edge.weight = intersection;
} }
}); });
} }
@@ -825,188 +835,200 @@ impl NarrowPhase {
// TODO: don't iterate on all the edges. // TODO: don't iterate on all the edges.
par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| { par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| {
let pair = &mut edge.weight; let pair = &mut edge.weight;
let had_any_active_contact = pair.has_any_active_contact;
let co_parent1: Option<&ColliderParent> = colliders.get(pair.collider1.0); // TODO: remove the `loop` once labels on blocks are supported.
let (co_changes1, co_shape1, co_pos1, co_material1, co_flags1): ( 'emit_events: loop {
&ColliderChanges, let co_parent1: Option<&ColliderParent> = colliders.get(pair.collider1.0);
&ColliderShape, let (co_changes1, co_shape1, co_pos1, co_material1, co_flags1): (
&ColliderPosition, &ColliderChanges,
&ColliderMaterial, &ColliderShape,
&ColliderFlags, &ColliderPosition,
) = colliders.index_bundle(pair.collider1.0); &ColliderMaterial,
&ColliderFlags,
) = colliders.index_bundle(pair.collider1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(pair.collider2.0); let co_parent2: Option<&ColliderParent> = colliders.get(pair.collider2.0);
let (co_changes2, co_shape2, co_pos2, co_material2, co_flags2): ( let (co_changes2, co_shape2, co_pos2, co_material2, co_flags2): (
&ColliderChanges, &ColliderChanges,
&ColliderShape, &ColliderShape,
&ColliderPosition, &ColliderPosition,
&ColliderMaterial, &ColliderMaterial,
&ColliderFlags, &ColliderFlags,
) = colliders.index_bundle(pair.collider2.0); ) = colliders.index_bundle(pair.collider2.0);
if !co_changes1.needs_narrow_phase_update() && !co_changes2.needs_narrow_phase_update() if !co_changes1.needs_narrow_phase_update()
{ && !co_changes2.needs_narrow_phase_update()
// No update needed for these colliders. {
return; // No update needed for these colliders.
}
// TODO: avoid lookup into bodies.
let mut rb_type1 = RigidBodyType::Static;
let mut rb_type2 = RigidBodyType::Static;
if let Some(co_parent1) = co_parent1 {
rb_type1 = *bodies.index(co_parent1.handle.0);
}
if let Some(co_parent2) = co_parent2 {
rb_type2 = *bodies.index(co_parent2.handle.0);
}
// Filter based on the rigid-body types.
if !co_flags1.active_collision_types.test(rb_type1, rb_type2)
&& !co_flags2.active_collision_types.test(rb_type1, rb_type2)
{
return;
}
// Filter based on collision groups.
if !co_flags1.collision_groups.test(co_flags2.collision_groups) {
return;
}
let active_hooks = co_flags1.active_hooks | co_flags2.active_hooks;
let active_events = co_flags1.active_events | co_flags2.active_events;
let mut solver_flags = if active_hooks.contains(ActiveHooks::FILTER_CONTACT_PAIRS) {
let context = PairFilterContext {
bodies,
colliders,
rigid_body1: co_parent1.map(|p| p.handle),
rigid_body2: co_parent2.map(|p| p.handle),
collider1: pair.collider1,
collider2: pair.collider2,
};
if let Some(solver_flags) = hooks.filter_contact_pair(&context) {
solver_flags
} else {
// No contact allowed.
return; return;
} }
} else {
SolverFlags::default()
};
if !co_flags1.solver_groups.test(co_flags2.solver_groups) { // TODO: avoid lookup into bodies.
solver_flags.remove(SolverFlags::COMPUTE_IMPULSES); let mut rb_type1 = RigidBodyType::Static;
} let mut rb_type2 = RigidBodyType::Static;
if co_changes1.contains(ColliderChanges::SHAPE) if let Some(co_parent1) = co_parent1 {
|| co_changes2.contains(ColliderChanges::SHAPE) rb_type1 = *bodies.index(co_parent1.handle.0);
{
// The shape changed so the workspace is no longer valid.
pair.workspace = None;
}
let pos12 = co_pos1.inv_mul(co_pos2);
let _ = query_dispatcher.contact_manifolds(
&pos12,
&**co_shape1,
&**co_shape2,
prediction_distance,
&mut pair.manifolds,
&mut pair.workspace,
);
let mut has_any_active_contact = false;
let friction = CoefficientCombineRule::combine(
co_material1.friction,
co_material2.friction,
co_material1.friction_combine_rule as u8,
co_material2.friction_combine_rule as u8,
);
let restitution = CoefficientCombineRule::combine(
co_material1.restitution,
co_material2.restitution,
co_material1.restitution_combine_rule as u8,
co_material2.restitution_combine_rule as u8,
);
let zero = RigidBodyDominance(0); // The value doesn't matter, it will be MAX because of the effective groups.
let dominance1 = co_parent1
.map(|p1| *bodies.index(p1.handle.0))
.unwrap_or(zero);
let dominance2 = co_parent2
.map(|p2| *bodies.index(p2.handle.0))
.unwrap_or(zero);
for manifold in &mut pair.manifolds {
let world_pos1 = manifold.subshape_pos1.prepend_to(co_pos1);
manifold.data.solver_contacts.clear();
manifold.data.rigid_body1 = co_parent1.map(|p| p.handle);
manifold.data.rigid_body2 = co_parent2.map(|p| p.handle);
manifold.data.solver_flags = solver_flags;
manifold.data.relative_dominance =
dominance1.effective_group(&rb_type1) - dominance2.effective_group(&rb_type2);
manifold.data.normal = world_pos1 * manifold.local_n1;
// 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."
);
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,
friction,
restitution,
tangent_velocity: Vector::zeros(),
is_new: contact.data.impulse == 0.0,
};
manifold.data.solver_contacts.push(solver_contact);
has_any_active_contact = true;
}
} }
// Apply the user-defined contact modification. if let Some(co_parent2) = co_parent2 {
if active_hooks.contains(ActiveHooks::MODIFY_SOLVER_CONTACTS) { rb_type2 = *bodies.index(co_parent2.handle.0);
let mut modifiable_solver_contacts = }
std::mem::replace(&mut manifold.data.solver_contacts, Vec::new());
let mut modifiable_user_data = manifold.data.user_data;
let mut modifiable_normal = manifold.data.normal;
let mut context = ContactModificationContext { // Filter based on the rigid-body types.
if !co_flags1.active_collision_types.test(rb_type1, rb_type2)
&& !co_flags2.active_collision_types.test(rb_type1, rb_type2)
{
pair.clear();
break 'emit_events;
}
// Filter based on collision groups.
if !co_flags1.collision_groups.test(co_flags2.collision_groups) {
pair.clear();
break 'emit_events;
}
let active_hooks = co_flags1.active_hooks | co_flags2.active_hooks;
let mut solver_flags = if active_hooks.contains(ActiveHooks::FILTER_CONTACT_PAIRS) {
let context = PairFilterContext {
bodies, bodies,
colliders, colliders,
rigid_body1: co_parent1.map(|p| p.handle), rigid_body1: co_parent1.map(|p| p.handle),
rigid_body2: co_parent2.map(|p| p.handle), rigid_body2: co_parent2.map(|p| p.handle),
collider1: pair.collider1, collider1: pair.collider1,
collider2: pair.collider2, collider2: pair.collider2,
manifold,
solver_contacts: &mut modifiable_solver_contacts,
normal: &mut modifiable_normal,
user_data: &mut modifiable_user_data,
}; };
hooks.modify_solver_contacts(&mut context); if let Some(solver_flags) = hooks.filter_contact_pair(&context) {
solver_flags
} else {
// No contact allowed.
pair.clear();
break 'emit_events;
}
} else {
SolverFlags::default()
};
manifold.data.solver_contacts = modifiable_solver_contacts; if !co_flags1.solver_groups.test(co_flags2.solver_groups) {
manifold.data.normal = modifiable_normal; solver_flags.remove(SolverFlags::COMPUTE_IMPULSES);
manifold.data.user_data = modifiable_user_data;
} }
if co_changes1.contains(ColliderChanges::SHAPE)
|| co_changes2.contains(ColliderChanges::SHAPE)
{
// The shape changed so the workspace is no longer valid.
pair.workspace = None;
}
let pos12 = co_pos1.inv_mul(co_pos2);
let _ = query_dispatcher.contact_manifolds(
&pos12,
&**co_shape1,
&**co_shape2,
prediction_distance,
&mut pair.manifolds,
&mut pair.workspace,
);
let friction = CoefficientCombineRule::combine(
co_material1.friction,
co_material2.friction,
co_material1.friction_combine_rule as u8,
co_material2.friction_combine_rule as u8,
);
let restitution = CoefficientCombineRule::combine(
co_material1.restitution,
co_material2.restitution,
co_material1.restitution_combine_rule as u8,
co_material2.restitution_combine_rule as u8,
);
let zero = RigidBodyDominance(0); // The value doesn't matter, it will be MAX because of the effective groups.
let dominance1 = co_parent1
.map(|p1| *bodies.index(p1.handle.0))
.unwrap_or(zero);
let dominance2 = co_parent2
.map(|p2| *bodies.index(p2.handle.0))
.unwrap_or(zero);
for manifold in &mut pair.manifolds {
let world_pos1 = manifold.subshape_pos1.prepend_to(co_pos1);
manifold.data.solver_contacts.clear();
manifold.data.rigid_body1 = co_parent1.map(|p| p.handle);
manifold.data.rigid_body2 = co_parent2.map(|p| p.handle);
manifold.data.solver_flags = solver_flags;
manifold.data.relative_dominance = dominance1.effective_group(&rb_type1)
- dominance2.effective_group(&rb_type2);
manifold.data.normal = world_pos1 * manifold.local_n1;
// Generate solver contacts.
pair.has_any_active_contact = false;
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."
);
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,
friction,
restitution,
tangent_velocity: Vector::zeros(),
is_new: contact.data.impulse == 0.0,
};
manifold.data.solver_contacts.push(solver_contact);
pair.has_any_active_contact = true;
}
}
// Apply the user-defined contact modification.
if active_hooks.contains(ActiveHooks::MODIFY_SOLVER_CONTACTS) {
let mut modifiable_solver_contacts =
std::mem::replace(&mut manifold.data.solver_contacts, Vec::new());
let mut modifiable_user_data = manifold.data.user_data;
let mut modifiable_normal = manifold.data.normal;
let mut context = ContactModificationContext {
bodies,
colliders,
rigid_body1: co_parent1.map(|p| p.handle),
rigid_body2: co_parent2.map(|p| p.handle),
collider1: pair.collider1,
collider2: pair.collider2,
manifold,
solver_contacts: &mut modifiable_solver_contacts,
normal: &mut modifiable_normal,
user_data: &mut modifiable_user_data,
};
hooks.modify_solver_contacts(&mut context);
manifold.data.solver_contacts = modifiable_solver_contacts;
manifold.data.normal = modifiable_normal;
manifold.data.user_data = modifiable_user_data;
}
}
break 'emit_events;
} }
if has_any_active_contact != pair.has_any_active_contact { let co_flags1: &ColliderFlags = colliders.index(pair.collider1.0);
let co_flags2: &ColliderFlags = colliders.index(pair.collider2.0);
let active_events = co_flags1.active_events | co_flags2.active_events;
if pair.has_any_active_contact != had_any_active_contact {
if active_events.contains(ActiveEvents::CONTACT_EVENTS) { if active_events.contains(ActiveEvents::CONTACT_EVENTS) {
if has_any_active_contact { if pair.has_any_active_contact {
events.handle_contact_event( events.handle_contact_event(
ContactEvent::Started(pair.collider1, pair.collider2), ContactEvent::Started(pair.collider1, pair.collider2),
pair, pair,
@@ -1018,8 +1040,6 @@ impl NarrowPhase {
); );
} }
} }
pair.has_any_active_contact = has_any_active_contact;
} }
}); });
} }

View File

@@ -682,6 +682,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
&mut self.harness.physics.colliders, &mut self.harness.physics.colliders,
&mut self.harness.physics.impulse_joints, &mut self.harness.physics.impulse_joints,
&mut self.harness.physics.multibody_joints, &mut self.harness.physics.multibody_joints,
true,
); );
} }
} }