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

Use newtypes for collider, rigid-body and joint handles.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-01-20 16:33:42 +01:00
parent 28b7866aee
commit 0ade350b5f
11 changed files with 224 additions and 175 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples2d/trimesh2.rs
View File

@@ -79,11 +79,7 @@ pub fn init_world(testbed: &mut Testbed) {
transform.get_scale().1 as f32 * 0.2, transform.get_scale().1 as f32 * 0.2,
); );
let indices: Vec<_> = mesh let indices: Vec<_> = mesh.indices.chunks(3).map(|v| [v[0], v[1], v[2]]).collect();
.indices
.chunks(3)
.map(|v| Point3::new(v[0], v[1], v[2]))
.collect();
let vertices: Vec<_> = mesh let vertices: Vec<_> = mesh
.vertices .vertices
.iter() .iter()

65
src/dynamics/joint/joint_set.rs
View File

@@ -1,11 +1,34 @@
use super::Joint; use super::Joint;
use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex}; use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
use crate::data::arena::{Arena, Index}; use crate::data::arena::Arena;
use crate::dynamics::{JointParams, RigidBodyHandle, RigidBodySet}; use crate::dynamics::{JointParams, RigidBodyHandle, RigidBodySet};
/// The unique identifier of a joint added to the joint set. /// The unique identifier of a joint added to the joint set.
pub type JointHandle = Index; /// The unique identifier of a collider added to a collider set.
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[repr(transparent)]
pub struct JointHandle(pub(crate) crate::data::arena::Index);
impl JointHandle {
pub fn into_raw_parts(self) -> (usize, u64) {
self.0.into_raw_parts()
}
pub fn from_raw_parts(id: usize, generation: u64) -> Self {
Self(crate::data::arena::Index::from_raw_parts(id, generation))
}
/// An always-invalid joint handle.
pub fn invalid() -> Self {
Self(crate::data::arena::Index::from_raw_parts(
crate::INVALID_USIZE,
crate::INVALID_U64,
))
}
}
pub(crate) type JointIndex = usize; pub(crate) type JointIndex = usize;
pub(crate) type JointGraphEdge = crate::data::graph::Edge<Joint>; pub(crate) type JointGraphEdge = crate::data::graph::Edge<Joint>;
@@ -13,7 +36,7 @@ pub(crate) type JointGraphEdge = crate::data::graph::Edge<Joint>;
/// A set of joints that can be handled by a physics `World`. /// A set of joints that can be handled by a physics `World`.
pub struct JointSet { pub struct JointSet {
joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph. joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph.
joint_graph: InteractionGraph<Joint>, joint_graph: InteractionGraph<RigidBodyHandle, Joint>,
} }
impl JointSet { impl JointSet {
@@ -25,29 +48,24 @@ impl JointSet {
} }
} }
/// An always-invalid joint handle.
pub fn invalid_handle() -> JointHandle {
JointHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64)
}
/// The number of joints on this set. /// The number of joints on this set.
pub fn len(&self) -> usize { pub fn len(&self) -> usize {
self.joint_graph.graph.edges.len() self.joint_graph.graph.edges.len()
} }
/// Retrieve the joint graph where edges are joints and nodes are rigid body handles. /// Retrieve the joint graph where edges are joints and nodes are rigid body handles.
pub fn joint_graph(&self) -> &InteractionGraph<Joint> { pub fn joint_graph(&self) -> &InteractionGraph<RigidBodyHandle, Joint> {
&self.joint_graph &self.joint_graph
} }
/// Is the given joint handle valid? /// Is the given joint handle valid?
pub fn contains(&self, handle: JointHandle) -> bool { pub fn contains(&self, handle: JointHandle) -> bool {
self.joint_ids.contains(handle) self.joint_ids.contains(handle.0)
} }
/// Gets the joint with the given handle. /// Gets the joint with the given handle.
pub fn get(&self, handle: JointHandle) -> Option<&Joint> { pub fn get(&self, handle: JointHandle) -> Option<&Joint> {
let id = self.joint_ids.get(handle)?; let id = self.joint_ids.get(handle.0)?;
self.joint_graph.graph.edge_weight(*id) self.joint_graph.graph.edge_weight(*id)
} }
@@ -62,7 +80,10 @@ impl JointSet {
/// suffer form the ABA problem. /// suffer form the ABA problem.
pub fn get_unknown_gen(&self, i: usize) -> Option<(&Joint, JointHandle)> { pub fn get_unknown_gen(&self, i: usize) -> Option<(&Joint, JointHandle)> {
let (id, handle) = self.joint_ids.get_unknown_gen(i)?; let (id, handle) = self.joint_ids.get_unknown_gen(i)?;
Some((self.joint_graph.graph.edge_weight(*id)?, handle)) Some((
self.joint_graph.graph.edge_weight(*id)?,
JointHandle(handle),
))
} }
/// Iterates through all the joint on this set. /// Iterates through all the joint on this set.
@@ -117,7 +138,7 @@ impl JointSet {
let joint = Joint { let joint = Joint {
body1, body1,
body2, body2,
handle, handle: JointHandle(handle),
#[cfg(feature = "parallel")] #[cfg(feature = "parallel")]
constraint_index: 0, constraint_index: 0,
#[cfg(feature = "parallel")] #[cfg(feature = "parallel")]
@@ -133,11 +154,13 @@ impl JointSet {
// NOTE: the body won't have a graph index if it does not // NOTE: the body won't have a graph index if it does not
// have any joint attached. // have any joint attached.
if !InteractionGraph::<Joint>::is_graph_index_valid(rb1.joint_graph_index) { if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(rb1.joint_graph_index)
{
rb1.joint_graph_index = self.joint_graph.graph.add_node(joint.body1); rb1.joint_graph_index = self.joint_graph.graph.add_node(joint.body1);
} }
if !InteractionGraph::<Joint>::is_graph_index_valid(rb2.joint_graph_index) { if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(rb2.joint_graph_index)
{
rb2.joint_graph_index = self.joint_graph.graph.add_node(joint.body2); rb2.joint_graph_index = self.joint_graph.graph.add_node(joint.body2);
} }
@@ -146,7 +169,7 @@ impl JointSet {
.add_edge(rb1.joint_graph_index, rb2.joint_graph_index, joint); .add_edge(rb1.joint_graph_index, rb2.joint_graph_index, joint);
self.joint_ids[handle] = id; self.joint_ids[handle] = id;
handle JointHandle(handle)
} }
/// Retrieve all the joints happening between two active bodies. /// Retrieve all the joints happening between two active bodies.
@@ -191,7 +214,7 @@ impl JointSet {
bodies: &mut RigidBodySet, bodies: &mut RigidBodySet,
wake_up: bool, wake_up: bool,
) -> Option<Joint> { ) -> Option<Joint> {
let id = self.joint_ids.remove(handle)?; let id = self.joint_ids.remove(handle.0)?;
let endpoints = self.joint_graph.graph.edge_endpoints(id)?; let endpoints = self.joint_graph.graph.edge_endpoints(id)?;
if wake_up { if wake_up {
@@ -207,7 +230,7 @@ impl JointSet {
let removed_joint = self.joint_graph.graph.remove_edge(id); let removed_joint = self.joint_graph.graph.remove_edge(id);
if let Some(edge) = self.joint_graph.graph.edge_weight(id) { if let Some(edge) = self.joint_graph.graph.edge_weight(id) {
self.joint_ids[edge.handle] = id; self.joint_ids[edge.handle.0] = id;
} }
removed_joint removed_joint
@@ -218,7 +241,7 @@ impl JointSet {
deleted_id: RigidBodyGraphIndex, deleted_id: RigidBodyGraphIndex,
bodies: &mut RigidBodySet, bodies: &mut RigidBodySet,
) { ) {
if InteractionGraph::<()>::is_graph_index_valid(deleted_id) { if InteractionGraph::<(), ()>::is_graph_index_valid(deleted_id) {
// We have to delete each joint one by one in order to: // We have to delete each joint one by one in order to:
// - Wake-up the attached bodies. // - Wake-up the attached bodies.
// - Update our Handle -> graph edge mapping. // - Update our Handle -> graph edge mapping.
@@ -229,12 +252,12 @@ impl JointSet {
.map(|e| (e.0, e.1, e.2.handle)) .map(|e| (e.0, e.1, e.2.handle))
.collect(); .collect();
for (h1, h2, to_delete_handle) in to_delete { for (h1, h2, to_delete_handle) in to_delete {
let to_delete_edge_id = self.joint_ids.remove(to_delete_handle).unwrap(); let to_delete_edge_id = self.joint_ids.remove(to_delete_handle.0).unwrap();
self.joint_graph.graph.remove_edge(to_delete_edge_id); self.joint_graph.graph.remove_edge(to_delete_edge_id);
// Update the id of the edge which took the place of the deleted one. // Update the id of the edge which took the place of the deleted one.
if let Some(j) = self.joint_graph.graph.edge_weight_mut(to_delete_edge_id) { if let Some(j) = self.joint_graph.graph.edge_weight_mut(to_delete_edge_id) {
self.joint_ids[j.handle] = to_delete_edge_id; self.joint_ids[j.handle.0] = to_delete_edge_id;
} }
// Wake up the attached bodies. // Wake up the attached bodies.

4
src/dynamics/rigid_body.rs
View File

@@ -108,7 +108,7 @@ impl RigidBody {
angular_damping: 0.0, angular_damping: 0.0,
colliders: Vec::new(), colliders: Vec::new(),
activation: ActivationStatus::new_active(), activation: ActivationStatus::new_active(),
joint_graph_index: InteractionGraph::<()>::invalid_graph_index(), joint_graph_index: InteractionGraph::<(), ()>::invalid_graph_index(),
active_island_id: 0, active_island_id: 0,
active_set_id: 0, active_set_id: 0,
active_set_offset: 0, active_set_offset: 0,
@@ -122,7 +122,7 @@ impl RigidBody {
pub(crate) fn reset_internal_references(&mut self) { pub(crate) fn reset_internal_references(&mut self) {
self.colliders = Vec::new(); self.colliders = Vec::new();
self.joint_graph_index = InteractionGraph::<()>::invalid_graph_index(); self.joint_graph_index = InteractionGraph::<(), ()>::invalid_graph_index();
self.active_island_id = 0; self.active_island_id = 0;
self.active_set_id = 0; self.active_set_id = 0;
self.active_set_offset = 0; self.active_set_offset = 0;

121
src/dynamics/rigid_body_set.rs
View File

@@ -3,11 +3,43 @@ use rayon::prelude::*;
use crate::data::arena::Arena; use crate::data::arena::Arena;
use crate::dynamics::{Joint, JointSet, RigidBody, RigidBodyChanges}; use crate::dynamics::{Joint, JointSet, RigidBody, RigidBodyChanges};
use crate::geometry::{ColliderHandle, ColliderSet, InteractionGraph, NarrowPhase}; use crate::geometry::{ColliderSet, InteractionGraph, NarrowPhase};
use cdl::partitioning::IndexedData;
use std::ops::{Index, IndexMut}; use std::ops::{Index, IndexMut};
/// The unique handle of a rigid body added to a `RigidBodySet`. /// The unique handle of a rigid body added to a `RigidBodySet`.
pub type RigidBodyHandle = crate::data::arena::Index; #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[repr(transparent)]
pub struct RigidBodyHandle(pub(crate) crate::data::arena::Index);
impl RigidBodyHandle {
pub fn into_raw_parts(self) -> (usize, u64) {
self.0.into_raw_parts()
}
pub fn from_raw_parts(id: usize, generation: u64) -> Self {
Self(crate::data::arena::Index::from_raw_parts(id, generation))
}
/// An always-invalid rigid-body handle.
pub fn invalid() -> Self {
Self(crate::data::arena::Index::from_raw_parts(
crate::INVALID_USIZE,
crate::INVALID_U64,
))
}
}
impl IndexedData for RigidBodyHandle {
fn default() -> Self {
Self(IndexedData::default())
}
fn index(&self) -> usize {
self.0.index()
}
}
#[derive(Copy, Clone, Debug, PartialEq, Eq)] #[derive(Copy, Clone, Debug, PartialEq, Eq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -66,11 +98,6 @@ impl RigidBodySet {
} }
} }
/// An always-invalid rigid-body handle.
pub fn invalid_handle() -> RigidBodyHandle {
RigidBodyHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64)
}
/// The number of rigid bodies on this set. /// The number of rigid bodies on this set.
pub fn len(&self) -> usize { pub fn len(&self) -> usize {
self.bodies.len() self.bodies.len()
@@ -78,7 +105,7 @@ impl RigidBodySet {
/// Is the given body handle valid? /// Is the given body handle valid?
pub fn contains(&self, handle: RigidBodyHandle) -> bool { pub fn contains(&self, handle: RigidBodyHandle) -> bool {
self.bodies.contains(handle) self.bodies.contains(handle.0)
} }
/// Insert a rigid body into this set and retrieve its handle. /// Insert a rigid body into this set and retrieve its handle.
@@ -88,10 +115,10 @@ impl RigidBodySet {
rb.reset_internal_references(); rb.reset_internal_references();
rb.changes.set(RigidBodyChanges::all(), true); rb.changes.set(RigidBodyChanges::all(), true);
let handle = self.bodies.insert(rb); let handle = RigidBodyHandle(self.bodies.insert(rb));
self.modified_bodies.push(handle); self.modified_bodies.push(handle);
let rb = &mut self.bodies[handle]; let rb = &mut self.bodies[handle.0];
if rb.is_kinematic() { if rb.is_kinematic() {
rb.active_set_id = self.active_kinematic_set.len(); rb.active_set_id = self.active_kinematic_set.len();
@@ -108,7 +135,7 @@ impl RigidBodySet {
colliders: &mut ColliderSet, colliders: &mut ColliderSet,
joints: &mut JointSet, joints: &mut JointSet,
) -> Option<RigidBody> { ) -> Option<RigidBody> {
let rb = self.bodies.remove(handle)?; let rb = self.bodies.remove(handle.0)?;
/* /*
* Update active sets. * Update active sets.
*/ */
@@ -119,7 +146,7 @@ impl RigidBodySet {
active_set.swap_remove(rb.active_set_id); active_set.swap_remove(rb.active_set_id);
if let Some(replacement) = active_set.get(rb.active_set_id) { if let Some(replacement) = active_set.get(rb.active_set_id) {
self.bodies[*replacement].active_set_id = rb.active_set_id; self.bodies[replacement.0].active_set_id = rb.active_set_id;
} }
} }
} }
@@ -148,7 +175,7 @@ impl RigidBodySet {
/// If `strong` is `true` then it is assured that the rigid-body will /// If `strong` is `true` then it is assured that the rigid-body will
/// remain awake during multiple subsequent timesteps. /// remain awake during multiple subsequent timesteps.
pub fn wake_up(&mut self, handle: RigidBodyHandle, strong: bool) { pub fn wake_up(&mut self, handle: RigidBodyHandle, strong: bool) {
if let Some(rb) = self.bodies.get_mut(handle) { if let Some(rb) = self.bodies.get_mut(handle.0) {
// TODO: what about kinematic bodies? // TODO: what about kinematic bodies?
if rb.is_dynamic() { if rb.is_dynamic() {
rb.wake_up(strong); rb.wake_up(strong);
@@ -171,7 +198,9 @@ impl RigidBodySet {
/// Using this is discouraged in favor of `self.get(handle)` which does not /// Using this is discouraged in favor of `self.get(handle)` which does not
/// suffer form the ABA problem. /// suffer form the ABA problem.
pub fn get_unknown_gen(&self, i: usize) -> Option<(&RigidBody, RigidBodyHandle)> { pub fn get_unknown_gen(&self, i: usize) -> Option<(&RigidBody, RigidBodyHandle)> {
self.bodies.get_unknown_gen(i) self.bodies
.get_unknown_gen(i)
.map(|(b, h)| (b, RigidBodyHandle(h)))
} }
/// Gets a mutable reference to the rigid-body with the given handle without a known generation. /// Gets a mutable reference to the rigid-body with the given handle without a known generation.
@@ -187,19 +216,19 @@ impl RigidBodySet {
let result = self.bodies.get_unknown_gen_mut(i)?; let result = self.bodies.get_unknown_gen_mut(i)?;
if !self.modified_all_bodies && !result.0.changes.contains(RigidBodyChanges::MODIFIED) { if !self.modified_all_bodies && !result.0.changes.contains(RigidBodyChanges::MODIFIED) {
result.0.changes = RigidBodyChanges::MODIFIED; result.0.changes = RigidBodyChanges::MODIFIED;
self.modified_bodies.push(result.1); self.modified_bodies.push(RigidBodyHandle(result.1));
} }
Some(result) Some((result.0, RigidBodyHandle(result.1)))
} }
/// Gets the rigid-body with the given handle. /// Gets the rigid-body with the given handle.
pub fn get(&self, handle: RigidBodyHandle) -> Option<&RigidBody> { pub fn get(&self, handle: RigidBodyHandle) -> Option<&RigidBody> {
self.bodies.get(handle) self.bodies.get(handle.0)
} }
/// Gets a mutable reference to the rigid-body with the given handle. /// Gets a mutable reference to the rigid-body with the given handle.
pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> { pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
let result = self.bodies.get_mut(handle)?; let result = self.bodies.get_mut(handle.0)?;
if !self.modified_all_bodies && !result.changes.contains(RigidBodyChanges::MODIFIED) { if !self.modified_all_bodies && !result.changes.contains(RigidBodyChanges::MODIFIED) {
result.changes = RigidBodyChanges::MODIFIED; result.changes = RigidBodyChanges::MODIFIED;
self.modified_bodies.push(handle); self.modified_bodies.push(handle);
@@ -208,7 +237,7 @@ impl RigidBodySet {
} }
pub(crate) fn get_mut_internal(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> { pub(crate) fn get_mut_internal(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
self.bodies.get_mut(handle) self.bodies.get_mut(handle.0)
} }
pub(crate) fn get2_mut_internal( pub(crate) fn get2_mut_internal(
@@ -216,19 +245,19 @@ impl RigidBodySet {
h1: RigidBodyHandle, h1: RigidBodyHandle,
h2: RigidBodyHandle, h2: RigidBodyHandle,
) -> (Option<&mut RigidBody>, Option<&mut RigidBody>) { ) -> (Option<&mut RigidBody>, Option<&mut RigidBody>) {
self.bodies.get2_mut(h1, h2) self.bodies.get2_mut(h1.0, h2.0)
} }
/// Iterates through all the rigid-bodies on this set. /// Iterates through all the rigid-bodies on this set.
pub fn iter(&self) -> impl Iterator<Item = (RigidBodyHandle, &RigidBody)> { pub fn iter(&self) -> impl Iterator<Item = (RigidBodyHandle, &RigidBody)> {
self.bodies.iter() self.bodies.iter().map(|(h, b)| (RigidBodyHandle(h), b))
} }
/// Iterates mutably through all the rigid-bodies on this set. /// Iterates mutably through all the rigid-bodies on this set.
pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, &mut RigidBody)> { pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, &mut RigidBody)> {
self.modified_bodies.clear(); self.modified_bodies.clear();
self.modified_all_bodies = true; self.modified_all_bodies = true;
self.bodies.iter_mut() self.bodies.iter_mut().map(|(h, b)| (RigidBodyHandle(h), b))
} }
/// Iter through all the active kinematic rigid-bodies on this set. /// Iter through all the active kinematic rigid-bodies on this set.
@@ -238,7 +267,7 @@ impl RigidBodySet {
let bodies: &'a _ = &self.bodies; let bodies: &'a _ = &self.bodies;
self.active_kinematic_set self.active_kinematic_set
.iter() .iter()
.filter_map(move |h| Some((*h, bodies.get(*h)?))) .filter_map(move |h| Some((*h, bodies.get(h.0)?)))
} }
/// Iter through all the active dynamic rigid-bodies on this set. /// Iter through all the active dynamic rigid-bodies on this set.
@@ -248,7 +277,7 @@ impl RigidBodySet {
let bodies: &'a _ = &self.bodies; let bodies: &'a _ = &self.bodies;
self.active_dynamic_set self.active_dynamic_set
.iter() .iter()
.filter_map(move |h| Some((*h, bodies.get(*h)?))) .filter_map(move |h| Some((*h, bodies.get(h.0)?)))
} }
#[cfg(not(feature = "parallel"))] #[cfg(not(feature = "parallel"))]
@@ -260,7 +289,7 @@ impl RigidBodySet {
let bodies: &'a _ = &self.bodies; let bodies: &'a _ = &self.bodies;
self.active_dynamic_set[island_range] self.active_dynamic_set[island_range]
.iter() .iter()
.filter_map(move |h| Some((*h, bodies.get(*h)?))) .filter_map(move |h| Some((*h, bodies.get(h.0)?)))
} }
#[inline(always)] #[inline(always)]
@@ -269,13 +298,13 @@ impl RigidBodySet {
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
) { ) {
for handle in &self.active_dynamic_set { for handle in &self.active_dynamic_set {
if let Some(rb) = self.bodies.get_mut(*handle) { if let Some(rb) = self.bodies.get_mut(handle.0) {
f(*handle, rb) f(*handle, rb)
} }
} }
for handle in &self.active_kinematic_set { for handle in &self.active_kinematic_set {
if let Some(rb) = self.bodies.get_mut(*handle) { if let Some(rb) = self.bodies.get_mut(handle.0) {
f(*handle, rb) f(*handle, rb)
} }
} }
@@ -287,7 +316,7 @@ impl RigidBodySet {
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
) { ) {
for handle in &self.active_dynamic_set { for handle in &self.active_dynamic_set {
if let Some(rb) = self.bodies.get_mut(*handle) { if let Some(rb) = self.bodies.get_mut(handle.0) {
f(*handle, rb) f(*handle, rb)
} }
} }
@@ -299,7 +328,7 @@ impl RigidBodySet {
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
) { ) {
for handle in &self.active_kinematic_set { for handle in &self.active_kinematic_set {
if let Some(rb) = self.bodies.get_mut(*handle) { if let Some(rb) = self.bodies.get_mut(handle.0) {
f(*handle, rb) f(*handle, rb)
} }
} }
@@ -314,7 +343,7 @@ impl RigidBodySet {
) { ) {
let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
for handle in &self.active_dynamic_set[island_range] { for handle in &self.active_dynamic_set[island_range] {
if let Some(rb) = self.bodies.get_mut(*handle) { if let Some(rb) = self.bodies.get_mut(handle.0) {
f(*handle, rb) f(*handle, rb)
} }
} }
@@ -338,7 +367,7 @@ impl RigidBodySet {
|| bodies.load(Ordering::Relaxed), || bodies.load(Ordering::Relaxed),
|bodies, handle| { |bodies, handle| {
let bodies: &mut Arena<RigidBody> = unsafe { std::mem::transmute(*bodies) }; let bodies: &mut Arena<RigidBody> = unsafe { std::mem::transmute(*bodies) };
if let Some(rb) = bodies.get_mut(*handle) { if let Some(rb) = bodies.get_mut(handle.0) {
f(*handle, rb) f(*handle, rb)
} }
}, },
@@ -401,7 +430,7 @@ impl RigidBodySet {
for (handle, rb) in self.bodies.iter_mut() { for (handle, rb) in self.bodies.iter_mut() {
Self::maintain_one( Self::maintain_one(
colliders, colliders,
handle, RigidBodyHandle(handle),
rb, rb,
&mut self.modified_inactive_set, &mut self.modified_inactive_set,
&mut self.active_kinematic_set, &mut self.active_kinematic_set,
@@ -413,7 +442,7 @@ impl RigidBodySet {
self.modified_all_bodies = false; self.modified_all_bodies = false;
} else { } else {
for handle in self.modified_bodies.drain(..) { for handle in self.modified_bodies.drain(..) {
if let Some(rb) = self.bodies.get_mut(handle) { if let Some(rb) = self.bodies.get_mut(handle.0) {
Self::maintain_one( Self::maintain_one(
colliders, colliders,
handle, handle,
@@ -431,7 +460,7 @@ impl RigidBodySet {
&mut self, &mut self,
colliders: &ColliderSet, colliders: &ColliderSet,
narrow_phase: &NarrowPhase, narrow_phase: &NarrowPhase,
joint_graph: &InteractionGraph<Joint>, joint_graph: &InteractionGraph<RigidBodyHandle, Joint>,
min_island_size: usize, min_island_size: usize,
) { ) {
assert!( assert!(
@@ -451,7 +480,7 @@ impl RigidBodySet {
// does not seem to affect performances nor stability. However it makes // does not seem to affect performances nor stability. However it makes
// debugging slightly nicer so we keep this rev. // debugging slightly nicer so we keep this rev.
for h in self.active_dynamic_set.drain(..).rev() { for h in self.active_dynamic_set.drain(..).rev() {
let rb = &mut self.bodies[h]; let rb = &mut self.bodies[h.0];
rb.update_energy(); rb.update_energy();
if rb.activation.energy <= rb.activation.threshold { if rb.activation.energy <= rb.activation.threshold {
// Mark them as sleeping for now. This will // Mark them as sleeping for now. This will
@@ -466,18 +495,18 @@ impl RigidBodySet {
// Read all the contacts and push objects touching touching this rigid-body. // Read all the contacts and push objects touching touching this rigid-body.
#[inline(always)] #[inline(always)]
fn push_contacting_colliders( fn push_contacting_bodies(
rb: &RigidBody, rb: &RigidBody,
colliders: &ColliderSet, colliders: &ColliderSet,
narrow_phase: &NarrowPhase, narrow_phase: &NarrowPhase,
stack: &mut Vec<ColliderHandle>, stack: &mut Vec<RigidBodyHandle>,
) { ) {
for collider_handle in &rb.colliders { for collider_handle in &rb.colliders {
if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) { if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) {
for inter in contacts { for inter in contacts {
for manifold in &inter.2.manifolds { for manifold in &inter.2.manifolds {
if !manifold.data.solver_contacts.is_empty() { if !manifold.data.solver_contacts.is_empty() {
let other = crate::utils::other_handle( let other = crate::utils::select_other(
(inter.0, inter.1), (inter.0, inter.1),
*collider_handle, *collider_handle,
); );
@@ -494,7 +523,7 @@ impl RigidBodySet {
// Now iterate on all active kinematic bodies and push all the bodies // Now iterate on all active kinematic bodies and push all the bodies
// touching them to the stack so they can be woken up. // touching them to the stack so they can be woken up.
for h in self.active_kinematic_set.iter() { for h in self.active_kinematic_set.iter() {
let rb = &self.bodies[*h]; let rb = &self.bodies[h.0];
if !rb.is_moving() { if !rb.is_moving() {
// If the kinematic body does not move, it does not have // If the kinematic body does not move, it does not have
@@ -502,7 +531,7 @@ impl RigidBodySet {
continue; continue;
} }
push_contacting_colliders(rb, colliders, narrow_phase, &mut self.stack); push_contacting_bodies(rb, colliders, narrow_phase, &mut self.stack);
} }
// println!("Selection: {}", instant::now() - t); // println!("Selection: {}", instant::now() - t);
@@ -517,7 +546,7 @@ impl RigidBodySet {
let mut island_marker = self.stack.len().max(1) - 1; let mut island_marker = self.stack.len().max(1) - 1;
while let Some(handle) = self.stack.pop() { while let Some(handle) = self.stack.pop() {
let rb = &mut self.bodies[handle]; let rb = &mut self.bodies[handle.0];
if rb.active_set_timestamp == self.active_set_timestamp || !rb.is_dynamic() { if rb.active_set_timestamp == self.active_set_timestamp || !rb.is_dynamic() {
// We already visited this body and its neighbors. // We already visited this body and its neighbors.
@@ -545,10 +574,10 @@ impl RigidBodySet {
// Transmit the active state to all the rigid-bodies with colliders // Transmit the active state to all the rigid-bodies with colliders
// in contact or joined with this collider. // in contact or joined with this collider.
push_contacting_colliders(rb, colliders, narrow_phase, &mut self.stack); push_contacting_bodies(rb, colliders, narrow_phase, &mut self.stack);
for inter in joint_graph.interactions_with(rb.joint_graph_index) { for inter in joint_graph.interactions_with(rb.joint_graph_index) {
let other = crate::utils::other_handle((inter.0, inter.1), handle); let other = crate::utils::select_other((inter.0, inter.1), handle);
self.stack.push(other); self.stack.push(other);
} }
} }
@@ -563,7 +592,7 @@ impl RigidBodySet {
// Actually put to sleep bodies which have not been detected as awake. // Actually put to sleep bodies which have not been detected as awake.
// let t = instant::now(); // let t = instant::now();
for h in &self.can_sleep { for h in &self.can_sleep {
let b = &mut self.bodies[*h]; let b = &mut self.bodies[h.0];
if b.activation.sleeping { if b.activation.sleeping {
b.sleep(); b.sleep();
} }
@@ -576,12 +605,12 @@ impl Index<RigidBodyHandle> for RigidBodySet {
type Output = RigidBody; type Output = RigidBody;
fn index(&self, index: RigidBodyHandle) -> &RigidBody { fn index(&self, index: RigidBodyHandle) -> &RigidBody {
&self.bodies[index] &self.bodies[index.0]
} }
} }
impl IndexMut<RigidBodyHandle> for RigidBodySet { impl IndexMut<RigidBodyHandle> for RigidBodySet {
fn index_mut(&mut self, index: RigidBodyHandle) -> &mut RigidBody { fn index_mut(&mut self, index: RigidBodyHandle) -> &mut RigidBody {
&mut self.bodies[index] &mut self.bodies[index.0]
} }
} }

4
src/dynamics/solver/parallel_island_solver.rs
View File

@@ -247,7 +247,7 @@ impl ParallelIslandSolver {
concurrent_loop! { concurrent_loop! {
let batch_size = thread.batch_size; let batch_size = thread.batch_size;
for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] { for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
let rb = &mut bodies[*handle]; let rb = &mut bodies[handle.0];
let dvel = mj_lambdas[rb.active_set_offset]; let dvel = mj_lambdas[rb.active_set_offset];
rb.linvel += dvel.linear; rb.linvel += dvel.linear;
rb.angvel += rb.world_inv_inertia_sqrt.transform_vector(dvel.angular); rb.angvel += rb.world_inv_inertia_sqrt.transform_vector(dvel.angular);
@@ -272,7 +272,7 @@ impl ParallelIslandSolver {
concurrent_loop! { concurrent_loop! {
let batch_size = thread.batch_size; let batch_size = thread.batch_size;
for handle in active_bodies[thread.position_writeback_index] { for handle in active_bodies[thread.position_writeback_index] {
let rb = &mut bodies[*handle]; let rb = &mut bodies[handle.0];
rb.set_position_internal(positions[rb.active_set_offset]); rb.set_position_internal(positions[rb.active_set_offset]);
} }
} }

6
src/geometry/collider.rs
View File

@@ -1,4 +1,4 @@
use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; use crate::dynamics::{MassProperties, RigidBodyHandle};
use crate::geometry::InteractionGroups; use crate::geometry::InteractionGroups;
use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector}; use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector};
use cdl::bounding_volume::AABB; use cdl::bounding_volume::AABB;
@@ -333,7 +333,7 @@ pub struct Collider {
impl Collider { impl Collider {
pub(crate) fn reset_internal_references(&mut self) { pub(crate) fn reset_internal_references(&mut self) {
self.parent = RigidBodySet::invalid_handle(); self.parent = RigidBodyHandle::invalid();
self.proxy_index = crate::INVALID_USIZE; self.proxy_index = crate::INVALID_USIZE;
} }
@@ -708,7 +708,7 @@ impl ColliderBuilder {
restitution: self.restitution, restitution: self.restitution,
delta: self.delta, delta: self.delta,
is_sensor: self.is_sensor, is_sensor: self.is_sensor,
parent: RigidBodySet::invalid_handle(), parent: RigidBodyHandle::invalid(),
position: Isometry::identity(), position: Isometry::identity(),
predicted_position: Isometry::identity(), predicted_position: Isometry::identity(),
proxy_index: crate::INVALID_USIZE, proxy_index: crate::INVALID_USIZE,

60
src/geometry/collider_set.rs
View File

@@ -2,10 +2,42 @@ use crate::data::arena::Arena;
use crate::data::pubsub::PubSub; use crate::data::pubsub::PubSub;
use crate::dynamics::{RigidBodyHandle, RigidBodySet}; use crate::dynamics::{RigidBodyHandle, RigidBodySet};
use crate::geometry::Collider; use crate::geometry::Collider;
use cdl::partitioning::IndexedData;
use std::ops::{Index, IndexMut}; use std::ops::{Index, IndexMut};
/// The unique identifier of a collider added to a collider set. /// The unique identifier of a collider added to a collider set.
pub type ColliderHandle = crate::data::arena::Index; #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[repr(transparent)]
pub struct ColliderHandle(pub(crate) crate::data::arena::Index);
impl ColliderHandle {
pub fn into_raw_parts(self) -> (usize, u64) {
self.0.into_raw_parts()
}
pub fn from_raw_parts(id: usize, generation: u64) -> Self {
Self(crate::data::arena::Index::from_raw_parts(id, generation))
}
/// An always-invalid collider handle.
pub fn invalid() -> Self {
Self(crate::data::arena::Index::from_raw_parts(
crate::INVALID_USIZE,
crate::INVALID_U64,
))
}
}
impl IndexedData for ColliderHandle {
fn default() -> Self {
Self(IndexedData::default())
}
fn index(&self) -> usize {
self.0.index()
}
}
#[derive(Copy, Clone, Debug)] #[derive(Copy, Clone, Debug)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -38,7 +70,7 @@ impl ColliderSet {
/// Iterate through all the colliders on this set. /// Iterate through all the colliders on this set.
pub fn iter(&self) -> impl ExactSizeIterator<Item = (ColliderHandle, &Collider)> { pub fn iter(&self) -> impl ExactSizeIterator<Item = (ColliderHandle, &Collider)> {
self.colliders.iter() self.colliders.iter().map(|(h, c)| (ColliderHandle(h), c))
} }
/// The number of colliders on this set. /// The number of colliders on this set.
@@ -48,7 +80,7 @@ impl ColliderSet {
/// Is this collider handle valid? /// Is this collider handle valid?
pub fn contains(&self, handle: ColliderHandle) -> bool { pub fn contains(&self, handle: ColliderHandle) -> bool {
self.colliders.contains(handle) self.colliders.contains(handle.0)
} }
/// Inserts a new collider to this set and retrieve its handle. /// Inserts a new collider to this set and retrieve its handle.
@@ -71,8 +103,8 @@ impl ColliderSet {
.expect("Parent rigid body not found."); .expect("Parent rigid body not found.");
coll.position = parent.position * coll.delta; coll.position = parent.position * coll.delta;
coll.predicted_position = parent.predicted_position * coll.delta; coll.predicted_position = parent.predicted_position * coll.delta;
let handle = self.colliders.insert(coll); let handle = ColliderHandle(self.colliders.insert(coll));
let coll = self.colliders.get(handle).unwrap(); let coll = self.colliders.get(handle.0).unwrap();
parent.add_collider(handle, &coll); parent.add_collider(handle, &coll);
handle handle
} }
@@ -87,7 +119,7 @@ impl ColliderSet {
bodies: &mut RigidBodySet, bodies: &mut RigidBodySet,
wake_up: bool, wake_up: bool,
) -> Option<Collider> { ) -> Option<Collider> {
let collider = self.colliders.remove(handle)?; let collider = self.colliders.remove(handle.0)?;
/* /*
* Delete the collider from its parent body. * Delete the collider from its parent body.
@@ -125,7 +157,9 @@ impl ColliderSet {
/// Using this is discouraged in favor of `self.get(handle)` which does not /// Using this is discouraged in favor of `self.get(handle)` which does not
/// suffer form the ABA problem. /// suffer form the ABA problem.
pub fn get_unknown_gen(&self, i: usize) -> Option<(&Collider, ColliderHandle)> { pub fn get_unknown_gen(&self, i: usize) -> Option<(&Collider, ColliderHandle)> {
self.colliders.get_unknown_gen(i) self.colliders
.get_unknown_gen(i)
.map(|(c, h)| (c, ColliderHandle(h)))
} }
/// Gets a mutable reference to the collider with the given handle without a known generation. /// Gets a mutable reference to the collider with the given handle without a known generation.
@@ -138,17 +172,19 @@ impl ColliderSet {
/// Using this is discouraged in favor of `self.get_mut(handle)` which does not /// Using this is discouraged in favor of `self.get_mut(handle)` which does not
/// suffer form the ABA problem. /// suffer form the ABA problem.
pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut Collider, ColliderHandle)> { pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut Collider, ColliderHandle)> {
self.colliders.get_unknown_gen_mut(i) self.colliders
.get_unknown_gen_mut(i)
.map(|(c, h)| (c, ColliderHandle(h)))
} }
/// Get the collider with the given handle. /// Get the collider with the given handle.
pub fn get(&self, handle: ColliderHandle) -> Option<&Collider> { pub fn get(&self, handle: ColliderHandle) -> Option<&Collider> {
self.colliders.get(handle) self.colliders.get(handle.0)
} }
/// Gets a mutable reference to the collider with the given handle. /// Gets a mutable reference to the collider with the given handle.
pub fn get_mut(&mut self, handle: ColliderHandle) -> Option<&mut Collider> { pub fn get_mut(&mut self, handle: ColliderHandle) -> Option<&mut Collider> {
self.colliders.get_mut(handle) self.colliders.get_mut(handle.0)
} }
// pub(crate) fn get2_mut_internal( // pub(crate) fn get2_mut_internal(
@@ -177,12 +213,12 @@ impl Index<ColliderHandle> for ColliderSet {
type Output = Collider; type Output = Collider;
fn index(&self, index: ColliderHandle) -> &Collider { fn index(&self, index: ColliderHandle) -> &Collider {
&self.colliders[index] &self.colliders[index.0]
} }
} }
impl IndexMut<ColliderHandle> for ColliderSet { impl IndexMut<ColliderHandle> for ColliderSet {
fn index_mut(&mut self, index: ColliderHandle) -> &mut Collider { fn index_mut(&mut self, index: ColliderHandle) -> &mut Collider {
&mut self.colliders[index] &mut self.colliders[index.0]
} }
} }

7
src/geometry/contact_pair.rs
View File

@@ -1,4 +1,4 @@
use crate::dynamics::{BodyPair, RigidBodySet}; use crate::dynamics::{BodyPair, RigidBodyHandle};
use crate::geometry::{ColliderPair, ContactManifold}; use crate::geometry::{ColliderPair, ContactManifold};
use crate::math::{Point, Real, Vector}; use crate::math::{Point, Real, Vector};
use cdl::query::ContactManifoldsWorkspace; use cdl::query::ContactManifoldsWorkspace;
@@ -113,10 +113,7 @@ pub struct SolverContact {
impl Default for ContactManifoldData { impl Default for ContactManifoldData {
fn default() -> Self { fn default() -> Self {
Self::new( Self::new(
BodyPair::new( BodyPair::new(RigidBodyHandle::invalid(), RigidBodyHandle::invalid()),
RigidBodySet::invalid_handle(),
RigidBodySet::invalid_handle(),
),
SolverFlags::empty(), SolverFlags::empty(),
) )
} }

72
src/geometry/interaction_graph.rs
View File

@@ -1,5 +1,4 @@
use crate::data::graph::{Direction, EdgeIndex, Graph, NodeIndex}; use crate::data::graph::{Direction, EdgeIndex, Graph, NodeIndex};
use crate::geometry::ColliderHandle;
/// Index of a node of the interaction graph. /// Index of a node of the interaction graph.
pub type ColliderGraphIndex = NodeIndex; pub type ColliderGraphIndex = NodeIndex;
@@ -11,11 +10,11 @@ pub type TemporaryInteractionIndex = EdgeIndex;
/// A graph where nodes are collision objects and edges are contact or proximity algorithms. /// A graph where nodes are collision objects and edges are contact or proximity algorithms.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)] #[derive(Clone)]
pub struct InteractionGraph<T> { pub struct InteractionGraph<N, E> {
pub(crate) graph: Graph<ColliderHandle, T>, pub(crate) graph: Graph<N, E>,
} }
impl<T> InteractionGraph<T> { impl<N: Copy, E> InteractionGraph<N, E> {
/// Creates a new empty collection of collision objects. /// Creates a new empty collection of collision objects.
pub fn new() -> Self { pub fn new() -> Self {
InteractionGraph { InteractionGraph {
@@ -24,7 +23,7 @@ impl<T> InteractionGraph<T> {
} }
/// The underlying raw graph structure of this interaction graph. /// The underlying raw graph structure of this interaction graph.
pub fn raw_graph(&self) -> &Graph<ColliderHandle, T> { pub fn raw_graph(&self) -> &Graph<N, E> {
&self.graph &self.graph
} }
@@ -40,7 +39,7 @@ impl<T> InteractionGraph<T> {
&mut self, &mut self,
index1: ColliderGraphIndex, index1: ColliderGraphIndex,
index2: ColliderGraphIndex, index2: ColliderGraphIndex,
interaction: T, interaction: E,
) -> TemporaryInteractionIndex { ) -> TemporaryInteractionIndex {
self.graph.add_edge(index1, index2, interaction) self.graph.add_edge(index1, index2, interaction)
} }
@@ -49,7 +48,7 @@ impl<T> InteractionGraph<T> {
&mut self, &mut self,
index1: ColliderGraphIndex, index1: ColliderGraphIndex,
index2: ColliderGraphIndex, index2: ColliderGraphIndex,
) -> Option<T> { ) -> Option<E> {
let id = self.graph.find_edge(index1, index2)?; let id = self.graph.find_edge(index1, index2)?;
self.graph.remove_edge(id) self.graph.remove_edge(id)
} }
@@ -69,20 +68,18 @@ impl<T> InteractionGraph<T> {
/// } /// }
/// ``` /// ```
#[must_use = "The graph index of the collision object returned by this method has been changed to `id`."] #[must_use = "The graph index of the collision object returned by this method has been changed to `id`."]
pub(crate) fn remove_node(&mut self, id: ColliderGraphIndex) -> Option<ColliderHandle> { pub(crate) fn remove_node(&mut self, id: ColliderGraphIndex) -> Option<N> {
let _ = self.graph.remove_node(id); let _ = self.graph.remove_node(id);
self.graph.node_weight(id).cloned() self.graph.node_weight(id).cloned()
} }
/// All the interactions on this graph. /// All the interactions on this graph.
pub fn interactions(&self) -> impl Iterator<Item = &T> { pub fn interactions(&self) -> impl Iterator<Item = &E> {
self.graph.raw_edges().iter().map(move |edge| &edge.weight) self.graph.raw_edges().iter().map(move |edge| &edge.weight)
} }
/// All the interactions on this graph with the corresponding endpoint weights. /// All the interactions on this graph with the corresponding endpoint weights.
pub fn interactions_with_endpoints( pub fn interactions_with_endpoints(&self) -> impl Iterator<Item = (N, N, &E)> {
&self,
) -> impl Iterator<Item = (ColliderHandle, ColliderHandle, &T)> {
self.graph.raw_edges().iter().map(move |edge| { self.graph.raw_edges().iter().map(move |edge| {
( (
self.graph.raw_nodes()[edge.source().index()].weight, self.graph.raw_nodes()[edge.source().index()].weight,
@@ -97,7 +94,7 @@ impl<T> InteractionGraph<T> {
&self, &self,
id1: ColliderGraphIndex, id1: ColliderGraphIndex,
id2: ColliderGraphIndex, id2: ColliderGraphIndex,
) -> Option<(ColliderHandle, ColliderHandle, &T)> { ) -> Option<(N, N, &E)> {
self.graph.find_edge(id1, id2).and_then(|edge| { self.graph.find_edge(id1, id2).and_then(|edge| {
let endpoints = self.graph.edge_endpoints(edge)?; let endpoints = self.graph.edge_endpoints(edge)?;
let h1 = self.graph.node_weight(endpoints.0)?; let h1 = self.graph.node_weight(endpoints.0)?;
@@ -112,7 +109,7 @@ impl<T> InteractionGraph<T> {
&mut self, &mut self,
id1: ColliderGraphIndex, id1: ColliderGraphIndex,
id2: ColliderGraphIndex, id2: ColliderGraphIndex,
) -> Option<(ColliderHandle, ColliderHandle, &mut T)> { ) -> Option<(N, N, &mut E)> {
let edge = self.graph.find_edge(id1, id2)?; let edge = self.graph.find_edge(id1, id2)?;
let endpoints = self.graph.edge_endpoints(edge)?; let endpoints = self.graph.edge_endpoints(edge)?;
let h1 = *self.graph.node_weight(endpoints.0)?; let h1 = *self.graph.node_weight(endpoints.0)?;
@@ -122,10 +119,7 @@ impl<T> InteractionGraph<T> {
} }
/// All the interaction involving the collision object with graph index `id`. /// All the interaction involving the collision object with graph index `id`.
pub fn interactions_with( pub fn interactions_with(&self, id: ColliderGraphIndex) -> impl Iterator<Item = (N, N, &E)> {
&self,
id: ColliderGraphIndex,
) -> impl Iterator<Item = (ColliderHandle, ColliderHandle, &T)> {
self.graph.edges(id).filter_map(move |e| { self.graph.edges(id).filter_map(move |e| {
let endpoints = self.graph.edge_endpoints(e.id()).unwrap(); let endpoints = self.graph.edge_endpoints(e.id()).unwrap();
Some((self.graph[endpoints.0], self.graph[endpoints.1], e.weight())) Some((self.graph[endpoints.0], self.graph[endpoints.1], e.weight()))
@@ -133,10 +127,7 @@ impl<T> InteractionGraph<T> {
} }
/// Gets the interaction with the given index. /// Gets the interaction with the given index.
pub fn index_interaction( pub fn index_interaction(&self, id: TemporaryInteractionIndex) -> Option<(N, N, &E)> {
&self,
id: TemporaryInteractionIndex,
) -> Option<(ColliderHandle, ColliderHandle, &T)> {
if let (Some(e), Some(endpoints)) = if let (Some(e), Some(endpoints)) =
(self.graph.edge_weight(id), self.graph.edge_endpoints(id)) (self.graph.edge_weight(id), self.graph.edge_endpoints(id))
{ {
@@ -150,14 +141,7 @@ impl<T> InteractionGraph<T> {
pub fn interactions_with_mut( pub fn interactions_with_mut(
&mut self, &mut self,
id: ColliderGraphIndex, id: ColliderGraphIndex,
) -> impl Iterator< ) -> impl Iterator<Item = (N, N, TemporaryInteractionIndex, &mut E)> {
Item = (
ColliderHandle,
ColliderHandle,
TemporaryInteractionIndex,
&mut T,
),
> {
let incoming_edge = self.graph.first_edge(id, Direction::Incoming); let incoming_edge = self.graph.first_edge(id, Direction::Incoming);
let outgoing_edge = self.graph.first_edge(id, Direction::Outgoing); let outgoing_edge = self.graph.first_edge(id, Direction::Outgoing);
@@ -172,7 +156,7 @@ impl<T> InteractionGraph<T> {
// pub fn colliders_interacting_with<'a>( // pub fn colliders_interacting_with<'a>(
// &'a self, // &'a self,
// id: ColliderGraphIndex, // id: ColliderGraphIndex,
// ) -> impl Iterator<Item = ColliderHandle> + 'a { // ) -> impl Iterator<Item = N> + 'a {
// self.graph.edges(id).filter_map(move |e| { // self.graph.edges(id).filter_map(move |e| {
// let inter = e.weight(); // let inter = e.weight();
// //
@@ -188,7 +172,7 @@ impl<T> InteractionGraph<T> {
// pub fn colliders_in_contact_with<'a>( // pub fn colliders_in_contact_with<'a>(
// &'a self, // &'a self,
// id: ColliderGraphIndex, // id: ColliderGraphIndex,
// ) -> impl Iterator<Item = ColliderHandle> + 'a { // ) -> impl Iterator<Item = N> + 'a {
// self.graph.edges(id).filter_map(move |e| { // self.graph.edges(id).filter_map(move |e| {
// let inter = e.weight(); // let inter = e.weight();
// //
@@ -209,7 +193,7 @@ impl<T> InteractionGraph<T> {
// pub fn colliders_in_proximity_of<'a>( // pub fn colliders_in_proximity_of<'a>(
// &'a self, // &'a self,
// id: ColliderGraphIndex, // id: ColliderGraphIndex,
// ) -> impl Iterator<Item = ColliderHandle> + 'a { // ) -> impl Iterator<Item = N> + 'a {
// self.graph.edges(id).filter_map(move |e| { // self.graph.edges(id).filter_map(move |e| {
// if let Interaction::Proximity(_, prox) = e.weight() { // if let Interaction::Proximity(_, prox) = e.weight() {
// if *prox == Proximity::Intersecting { // if *prox == Proximity::Intersecting {
@@ -226,29 +210,17 @@ impl<T> InteractionGraph<T> {
// } // }
} }
pub struct InteractionsWithMut<'a, T> { pub struct InteractionsWithMut<'a, N, E> {
graph: &'a mut Graph<ColliderHandle, T>, graph: &'a mut Graph<N, E>,
incoming_edge: Option<EdgeIndex>, incoming_edge: Option<EdgeIndex>,
outgoing_edge: Option<EdgeIndex>, outgoing_edge: Option<EdgeIndex>,
} }
impl<'a, T> Iterator for InteractionsWithMut<'a, T> { impl<'a, N: Copy, E> Iterator for InteractionsWithMut<'a, N, E> {
type Item = ( type Item = (N, N, TemporaryInteractionIndex, &'a mut E);
ColliderHandle,
ColliderHandle,
TemporaryInteractionIndex,
&'a mut T,
);
#[inline] #[inline]
fn next( fn next(&mut self) -> Option<(N, N, TemporaryInteractionIndex, &'a mut E)> {
&mut self,
) -> Option<(
ColliderHandle,
ColliderHandle,
TemporaryInteractionIndex,
&'a mut T,
)> {
if let Some(edge) = self.incoming_edge { if let Some(edge) = self.incoming_edge {
self.incoming_edge = self.graph.next_edge(edge, Direction::Incoming); self.incoming_edge = self.graph.next_edge(edge, Direction::Incoming);
let endpoints = self.graph.edge_endpoints(edge).unwrap(); let endpoints = self.graph.edge_endpoints(edge).unwrap();

46
src/geometry/narrow_phase.rs
View File

@@ -27,8 +27,8 @@ struct ColliderGraphIndices {
impl ColliderGraphIndices { impl ColliderGraphIndices {
fn invalid() -> Self { fn invalid() -> Self {
Self { Self {
contact_graph_index: InteractionGraph::<ContactPair>::invalid_graph_index(), contact_graph_index: InteractionGraph::<(), ()>::invalid_graph_index(),
intersection_graph_index: InteractionGraph::<bool>::invalid_graph_index(), intersection_graph_index: InteractionGraph::<(), ()>::invalid_graph_index(),
} }
} }
} }
@@ -42,8 +42,8 @@ pub struct NarrowPhase {
serde(skip, default = "crate::geometry::default_persistent_query_dispatcher") serde(skip, default = "crate::geometry::default_persistent_query_dispatcher")
)] )]
query_dispatcher: Arc<dyn PersistentQueryDispatcher<ContactManifoldData, ContactData>>, query_dispatcher: Arc<dyn PersistentQueryDispatcher<ContactManifoldData, ContactData>>,
contact_graph: InteractionGraph<ContactPair>, contact_graph: InteractionGraph<ColliderHandle, ContactPair>,
intersection_graph: InteractionGraph<bool>, intersection_graph: InteractionGraph<ColliderHandle, bool>,
graph_indices: Coarena<ColliderGraphIndices>, graph_indices: Coarena<ColliderGraphIndices>,
removed_colliders: Option<Subscription<RemovedCollider>>, removed_colliders: Option<Subscription<RemovedCollider>>,
} }
@@ -71,12 +71,12 @@ impl NarrowPhase {
} }
/// The contact graph containing all contact pairs and their contact information. /// The contact graph containing all contact pairs and their contact information.
pub fn contact_graph(&self) -> &InteractionGraph<ContactPair> { pub fn contact_graph(&self) -> &InteractionGraph<ColliderHandle, ContactPair> {
&self.contact_graph &self.contact_graph
} }
/// The intersection graph containing all intersection pairs and their intersection information. /// The intersection graph containing all intersection pairs and their intersection information.
pub fn intersection_graph(&self) -> &InteractionGraph<bool> { pub fn intersection_graph(&self) -> &InteractionGraph<ColliderHandle, bool> {
&self.intersection_graph &self.intersection_graph
} }
@@ -85,7 +85,7 @@ impl NarrowPhase {
&self, &self,
collider: ColliderHandle, collider: ColliderHandle,
) -> Option<impl Iterator<Item = (ColliderHandle, ColliderHandle, &ContactPair)>> { ) -> Option<impl Iterator<Item = (ColliderHandle, ColliderHandle, &ContactPair)>> {
let id = self.graph_indices.get(collider)?; let id = self.graph_indices.get(collider.0)?;
Some(self.contact_graph.interactions_with(id.contact_graph_index)) Some(self.contact_graph.interactions_with(id.contact_graph_index))
} }
@@ -94,7 +94,7 @@ impl NarrowPhase {
&'a self, &'a self,
collider: ColliderHandle, collider: ColliderHandle,
) -> Option<impl Iterator<Item = (ColliderHandle, ColliderHandle, bool)> + 'a> { ) -> Option<impl Iterator<Item = (ColliderHandle, ColliderHandle, bool)> + 'a> {
let id = self.graph_indices.get(collider)?; let id = self.graph_indices.get(collider.0)?;
Some( Some(
self.intersection_graph self.intersection_graph
.interactions_with(id.intersection_graph_index) .interactions_with(id.intersection_graph_index)
@@ -112,8 +112,8 @@ impl NarrowPhase {
collider1: ColliderHandle, collider1: ColliderHandle,
collider2: ColliderHandle, collider2: ColliderHandle,
) -> Option<&ContactPair> { ) -> Option<&ContactPair> {
let id1 = self.graph_indices.get(collider1)?; let id1 = self.graph_indices.get(collider1.0)?;
let id2 = self.graph_indices.get(collider2)?; let id2 = self.graph_indices.get(collider2.0)?;
self.contact_graph self.contact_graph
.interaction_pair(id1.contact_graph_index, id2.contact_graph_index) .interaction_pair(id1.contact_graph_index, id2.contact_graph_index)
.map(|c| c.2) .map(|c| c.2)
@@ -128,8 +128,8 @@ impl NarrowPhase {
collider1: ColliderHandle, collider1: ColliderHandle,
collider2: ColliderHandle, collider2: ColliderHandle,
) -> Option<bool> { ) -> Option<bool> {
let id1 = self.graph_indices.get(collider1)?; let id1 = self.graph_indices.get(collider1.0)?;
let id2 = self.graph_indices.get(collider2)?; let id2 = self.graph_indices.get(collider2.0)?;
self.intersection_graph self.intersection_graph
.interaction_pair(id1.intersection_graph_index, id2.intersection_graph_index) .interaction_pair(id1.intersection_graph_index, id2.intersection_graph_index)
.map(|c| *c.2) .map(|c| *c.2)
@@ -173,7 +173,7 @@ impl NarrowPhase {
while let Some(collider) = colliders.removed_colliders.read_ith(&cursor, i) { while let Some(collider) = colliders.removed_colliders.read_ith(&cursor, i) {
// NOTE: if the collider does not have any graph indices currently, there is nothing // NOTE: if the collider does not have any graph indices currently, there is nothing
// to remove in the narrow-phase for this collider. // to remove in the narrow-phase for this collider.
if let Some(graph_idx) = self.graph_indices.get(collider.handle) { if let Some(graph_idx) = self.graph_indices.get(collider.handle.0) {
let intersection_graph_id = prox_id_remap let intersection_graph_id = prox_id_remap
.get(&collider.handle) .get(&collider.handle)
.copied() .copied()
@@ -223,7 +223,7 @@ impl NarrowPhase {
// We have to manage the fact that one other collider will // We have to manage the fact that one other collider will
// have its graph index changed because of the node's swap-remove. // have its graph index changed because of the node's swap-remove.
if let Some(replacement) = self.intersection_graph.remove_node(intersection_graph_id) { if let Some(replacement) = self.intersection_graph.remove_node(intersection_graph_id) {
if let Some(replacement) = self.graph_indices.get_mut(replacement) { if let Some(replacement) = self.graph_indices.get_mut(replacement.0) {
replacement.intersection_graph_index = intersection_graph_id; replacement.intersection_graph_index = intersection_graph_id;
} else { } else {
prox_id_remap.insert(replacement, intersection_graph_id); prox_id_remap.insert(replacement, intersection_graph_id);
@@ -231,7 +231,7 @@ impl NarrowPhase {
} }
if let Some(replacement) = self.contact_graph.remove_node(contact_graph_id) { if let Some(replacement) = self.contact_graph.remove_node(contact_graph_id) {
if let Some(replacement) = self.graph_indices.get_mut(replacement) { if let Some(replacement) = self.graph_indices.get_mut(replacement.0) {
replacement.contact_graph_index = contact_graph_id; replacement.contact_graph_index = contact_graph_id;
} else { } else {
contact_id_remap.insert(replacement, contact_graph_id); contact_id_remap.insert(replacement, contact_graph_id);
@@ -258,22 +258,22 @@ impl NarrowPhase {
} }
let (gid1, gid2) = self.graph_indices.ensure_pair_exists( let (gid1, gid2) = self.graph_indices.ensure_pair_exists(
pair.collider1, pair.collider1.0,
pair.collider2, pair.collider2.0,
ColliderGraphIndices::invalid(), ColliderGraphIndices::invalid(),
); );
if co1.is_sensor() || co2.is_sensor() { if co1.is_sensor() || co2.is_sensor() {
// NOTE: the collider won't have a graph index as long // NOTE: the collider won't have a graph index as long
// as it does not interact with anything. // as it does not interact with anything.
if !InteractionGraph::<bool>::is_graph_index_valid( if !InteractionGraph::<(), ()>::is_graph_index_valid(
gid1.intersection_graph_index, gid1.intersection_graph_index,
) { ) {
gid1.intersection_graph_index = gid1.intersection_graph_index =
self.intersection_graph.graph.add_node(pair.collider1); self.intersection_graph.graph.add_node(pair.collider1);
} }
if !InteractionGraph::<bool>::is_graph_index_valid( if !InteractionGraph::<(), ()>::is_graph_index_valid(
gid2.intersection_graph_index, gid2.intersection_graph_index,
) { ) {
gid2.intersection_graph_index = gid2.intersection_graph_index =
@@ -301,14 +301,14 @@ impl NarrowPhase {
// NOTE: the collider won't have a graph index as long // NOTE: the collider won't have a graph index as long
// as it does not interact with anything. // as it does not interact with anything.
if !InteractionGraph::<ContactPair>::is_graph_index_valid( if !InteractionGraph::<(), ()>::is_graph_index_valid(
gid1.contact_graph_index, gid1.contact_graph_index,
) { ) {
gid1.contact_graph_index = gid1.contact_graph_index =
self.contact_graph.graph.add_node(pair.collider1); self.contact_graph.graph.add_node(pair.collider1);
} }
if !InteractionGraph::<ContactPair>::is_graph_index_valid( if !InteractionGraph::<(), ()>::is_graph_index_valid(
gid2.contact_graph_index, gid2.contact_graph_index,
) { ) {
gid2.contact_graph_index = gid2.contact_graph_index =
@@ -338,8 +338,8 @@ impl NarrowPhase {
// TODO: could we just unwrap here? // TODO: could we just unwrap here?
// Don't we have the guarantee that we will get a `AddPair` before a `DeletePair`? // Don't we have the guarantee that we will get a `AddPair` before a `DeletePair`?
if let (Some(gid1), Some(gid2)) = ( if let (Some(gid1), Some(gid2)) = (
self.graph_indices.get(pair.collider1), self.graph_indices.get(pair.collider1.0),
self.graph_indices.get(pair.collider2), self.graph_indices.get(pair.collider2.0),
) { ) {
if co1.is_sensor() || co2.is_sensor() { if co1.is_sensor() || co2.is_sensor() {
let was_intersecting = self.intersection_graph.remove_edge( let was_intersecting = self.intersection_graph.remove_edge(

8
src/utils.rs
View File

@@ -1,6 +1,5 @@
//! Miscellaneous utilities. //! Miscellaneous utilities.
use crate::dynamics::RigidBodyHandle;
use na::{Matrix3, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3}; use na::{Matrix3, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3};
use num::Zero; use num::Zero;
use simba::simd::SimdValue; use simba::simd::SimdValue;
@@ -658,11 +657,8 @@ impl Drop for FlushToZeroDenormalsAreZeroFlags {
} }
} }
pub(crate) fn other_handle( pub(crate) fn select_other<T: PartialEq>(pair: (T, T), elt: T) -> T {
pair: (RigidBodyHandle, RigidBodyHandle), if pair.0 == elt {
handle: RigidBodyHandle,
) -> RigidBodyHandle {
if pair.0 == handle {
pair.1 pair.1
} else { } else {
pair.0 pair.0