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

Merge branch 'master' into infinite_fall_memory

Browse Source
This commit is contained in:
Robert Hrusecky
2020-11-02 15:00:12 -06:00
parent bcec54ef31 a38fdc101d
commit 8b81a3fd27
21 changed files with 446 additions and 206 deletions
Download Patch File Download Diff File Expand all files Collapse all files

137
src/data/hashmap.rs Normal file
View File

@@ -0,0 +1,137 @@
//! A hash-map that behaves deterministically when the
//! `enhanced-determinism` feature is enabled.
#[cfg(all(feature = "enhanced-determinism", feature = "serde-serialize"))]
use indexmap::IndexMap as StdHashMap;
#[cfg(all(not(feature = "enhanced-determinism"), feature = "serde-serialize"))]
use std::collections::HashMap as StdHashMap;
/// Serializes only the capacity of a hash-map instead of its actual content.
#[cfg(feature = "serde-serialize")]
pub fn serialize_hashmap_capacity<S: serde::Serializer, K, V, H: std::hash::BuildHasher>(
map: &StdHashMap<K, V, H>,
s: S,
) -> Result<S::Ok, S::Error> {
s.serialize_u64(map.capacity() as u64)
}
/// Creates a new hash-map with its capacity deserialized from `d`.
#[cfg(feature = "serde-serialize")]
pub fn deserialize_hashmap_capacity<
'de,
D: serde::Deserializer<'de>,
K,
V,
H: std::hash::BuildHasher + Default,
>(
d: D,
) -> Result<StdHashMap<K, V, H>, D::Error> {
struct CapacityVisitor;
impl<'de> serde::de::Visitor<'de> for CapacityVisitor {
type Value = u64;
fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result {
write!(formatter, "an integer between 0 and 2^64")
}
fn visit_u64<E: serde::de::Error>(self, val: u64) -> Result<Self::Value, E> {
Ok(val)
}
}
let capacity = d.deserialize_u64(CapacityVisitor)? as usize;
Ok(StdHashMap::with_capacity_and_hasher(
capacity,
Default::default(),
))
}
/*
* FxHasher taken from rustc_hash, except that it does not depend on the pointer size.
*/
#[cfg(feature = "enhanced-determinism")]
pub type FxHashMap32<K, V> = indexmap::IndexMap<K, V, std::hash::BuildHasherDefault<FxHasher32>>;
#[cfg(feature = "enhanced-determinism")]
pub use {self::FxHashMap32 as HashMap, indexmap::map::Entry};
#[cfg(not(feature = "enhanced-determinism"))]
pub use {rustc_hash::FxHashMap as HashMap, std::collections::hash_map::Entry};
const K: u32 = 0x9e3779b9;
// Same as FxHasher, but with the guarantee that the internal hash is
// an u32 instead of something that depends on the platform.
pub struct FxHasher32 {
hash: u32,
}
impl Default for FxHasher32 {
#[inline]
fn default() -> FxHasher32 {
FxHasher32 { hash: 0 }
}
}
impl FxHasher32 {
#[inline]
fn add_to_hash(&mut self, i: u32) {
use std::ops::BitXor;
self.hash = self.hash.rotate_left(5).bitxor(i).wrapping_mul(K);
}
}
impl std::hash::Hasher for FxHasher32 {
#[inline]
fn write(&mut self, mut bytes: &[u8]) {
use std::convert::TryInto;
let read_u32 = |bytes: &[u8]| u32::from_ne_bytes(bytes[..4].try_into().unwrap());
let mut hash = FxHasher32 { hash: self.hash };
assert!(std::mem::size_of::<u32>() <= 8);
while bytes.len() >= std::mem::size_of::<u32>() {
hash.add_to_hash(read_u32(bytes) as u32);
bytes = &bytes[std::mem::size_of::<u32>()..];
}
if (std::mem::size_of::<u32>() > 4) && (bytes.len() >= 4) {
hash.add_to_hash(u32::from_ne_bytes(bytes[..4].try_into().unwrap()) as u32);
bytes = &bytes[4..];
}
if (std::mem::size_of::<u32>() > 2) && bytes.len() >= 2 {
hash.add_to_hash(u16::from_ne_bytes(bytes[..2].try_into().unwrap()) as u32);
bytes = &bytes[2..];
}
if (std::mem::size_of::<u32>() > 1) && bytes.len() >= 1 {
hash.add_to_hash(bytes[0] as u32);
}
self.hash = hash.hash;
}
#[inline]
fn write_u8(&mut self, i: u8) {
self.add_to_hash(i as u32);
}
#[inline]
fn write_u16(&mut self, i: u16) {
self.add_to_hash(i as u32);
}
#[inline]
fn write_u32(&mut self, i: u32) {
self.add_to_hash(i as u32);
}
#[inline]
fn write_u64(&mut self, i: u64) {
self.add_to_hash(i as u32);
self.add_to_hash((i >> 32) as u32);
}
#[inline]
fn write_usize(&mut self, i: usize) {
self.add_to_hash(i as u32);
}
#[inline]
fn finish(&self) -> u64 {
self.hash as u64
}
}

14
src/data/maybe_serializable_data.rs Normal file
View File

@@ -0,0 +1,14 @@
use downcast_rs::{impl_downcast, DowncastSync};
#[cfg(feature = "serde-serialize")]
use erased_serde::Serialize;
/// Piece of data that may be serializable.
pub trait MaybeSerializableData: DowncastSync {
/// Convert this shape as a serializable entity.
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> {
None
}
}
impl_downcast!(sync MaybeSerializableData);

4
src/data/mod.rs
View File

@@ -1,5 +1,9 @@
//! Data structures modified with guaranteed deterministic behavior after deserialization.
pub use self::maybe_serializable_data::MaybeSerializableData;
pub mod arena;
pub(crate) mod graph;
pub(crate) mod hashmap;
mod maybe_serializable_data;
pub mod pubsub;

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

@@ -181,6 +181,38 @@ impl JointSet {
}
}
/// Removes a joint from this set.
///
/// If `wake_up` is set to `true`, then the bodies attached to this joint will be
/// automatically woken up.
pub fn remove(
&mut self,
handle: JointHandle,
bodies: &mut RigidBodySet,
wake_up: bool,
) -> Option<Joint> {
let id = self.joint_ids.remove(handle)?;
let endpoints = self.joint_graph.graph.edge_endpoints(id)?;
if wake_up {
// Wake-up the bodies attached to this joint.
if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.0) {
bodies.wake_up(*rb_handle, true);
}
if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.1) {
bodies.wake_up(*rb_handle, true);
}
}
let removed_joint = self.joint_graph.graph.remove_edge(id);
if let Some(edge) = self.joint_graph.graph.edge_weight(id) {
self.joint_ids[edge.handle] = id;
}
removed_joint
}
pub(crate) fn remove_rigid_body(
&mut self,
deleted_id: RigidBodyGraphIndex,

9
src/geometry/broad_phase_multi_sap.rs
View File

@@ -1,13 +1,10 @@
use crate::data::hashmap::HashMap;
use crate::data::pubsub::Subscription;
use crate::dynamics::RigidBodySet;
use crate::geometry::{ColliderHandle, ColliderSet, RemovedCollider};
use crate::math::{Point, Vector, DIM};
#[cfg(feature = "enhanced-determinism")]
use crate::utils::FxHashMap32 as HashMap;
use bit_vec::BitVec;
use ncollide::bounding_volume::{BoundingVolume, AABB};
#[cfg(not(feature = "enhanced-determinism"))]
use rustc_hash::FxHashMap as HashMap;
use std::cmp::Ordering;
use std::ops::{Index, IndexMut};
@@ -444,8 +441,8 @@ pub struct BroadPhase {
#[cfg_attr(
feature = "serde-serialize",
serde(
serialize_with = "crate::utils::serialize_hashmap_capacity",
deserialize_with = "crate::utils::deserialize_hashmap_capacity"
serialize_with = "crate::data::hashmap::serialize_hashmap_capacity",
deserialize_with = "crate::data::hashmap::deserialize_hashmap_capacity"
)
)]
reporting: HashMap<(u32, u32), bool>, // Workspace

15
src/geometry/contact.rs
View File

@@ -1,8 +1,8 @@
use crate::data::MaybeSerializableData;
use crate::dynamics::BodyPair;
use crate::geometry::contact_generator::ContactPhase;
use crate::geometry::contact_generator::{ContactGeneratorWorkspace, ContactPhase};
use crate::geometry::{Collider, ColliderPair, ColliderSet};
use crate::math::{Isometry, Point, Vector};
use std::any::Any;
#[cfg(feature = "simd-is-enabled")]
use {
crate::math::{SimdFloat, SIMD_WIDTH},
@@ -182,15 +182,14 @@ pub struct ContactPair {
pub manifolds: Vec<ContactManifold>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
pub(crate) generator: Option<ContactPhase>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
pub(crate) generator_workspace: Option<Box<dyn Any + Send + Sync>>,
pub(crate) generator_workspace: Option<ContactGeneratorWorkspace>,
}
impl ContactPair {
pub(crate) fn new(
pair: ColliderPair,
generator: ContactPhase,
generator_workspace: Option<Box<dyn Any + Send + Sync>>,
generator_workspace: Option<ContactGeneratorWorkspace>,
) -> Self {
Self {
pair,
@@ -221,7 +220,7 @@ impl ContactPair {
&'b Collider,
&'b Collider,
&'a mut ContactManifold,
Option<&'a mut (dyn Any + Send + Sync)>,
Option<&'a mut (dyn MaybeSerializableData)>,
) {
let coll1 = &colliders[self.pair.collider1];
let coll2 = &colliders[self.pair.collider2];
@@ -240,14 +239,14 @@ impl ContactPair {
coll1,
coll2,
manifold,
self.generator_workspace.as_mut().map(|w| &mut **w),
self.generator_workspace.as_mut().map(|w| &mut *w.0),
)
} else {
(
coll2,
coll1,
manifold,
self.generator_workspace.as_mut().map(|w| &mut **w),
self.generator_workspace.as_mut().map(|w| &mut *w.0),
)
}
}

32
src/geometry/contact_generator/contact_dispatcher.rs
View File

@@ -1,11 +1,11 @@
#[cfg(feature = "dim3")]
use crate::geometry::contact_generator::PfmPfmContactManifoldGeneratorWorkspace;
use crate::geometry::contact_generator::{
ContactGenerator, ContactPhase, HeightFieldShapeContactGeneratorWorkspace,
PrimitiveContactGenerator, TrimeshShapeContactGeneratorWorkspace,
ContactGenerator, ContactGeneratorWorkspace, ContactPhase,
HeightFieldShapeContactGeneratorWorkspace, PrimitiveContactGenerator,
TrimeshShapeContactGeneratorWorkspace,
};
use crate::geometry::ShapeType;
use std::any::Any;
/// Trait implemented by structures responsible for selecting a collision-detection algorithm
/// for a given pair of shapes.
@@ -15,16 +15,13 @@ pub trait ContactDispatcher {
&self,
shape1: ShapeType,
shape2: ShapeType,
) -> (
PrimitiveContactGenerator,
Option<Box<dyn Any + Send + Sync>>,
);
) -> (PrimitiveContactGenerator, Option<ContactGeneratorWorkspace>);
/// Select the collision-detection algorithm for the given pair of non-primitive shapes.
fn dispatch(
&self,
shape1: ShapeType,
shape2: ShapeType,
) -> (ContactPhase, Option<Box<dyn Any + Send + Sync>>);
) -> (ContactPhase, Option<ContactGeneratorWorkspace>);
}
/// The default contact dispatcher used by Rapier.
@@ -35,10 +32,7 @@ impl ContactDispatcher for DefaultContactDispatcher {
&self,
shape1: ShapeType,
shape2: ShapeType,
) -> (
PrimitiveContactGenerator,
Option<Box<dyn Any + Send + Sync>>,
) {
) -> (PrimitiveContactGenerator, Option<ContactGeneratorWorkspace>) {
match (shape1, shape2) {
(ShapeType::Ball, ShapeType::Ball) => (
PrimitiveContactGenerator {
@@ -106,7 +100,9 @@ impl ContactDispatcher for DefaultContactDispatcher {
generate_contacts: super::generate_contacts_pfm_pfm,
..PrimitiveContactGenerator::default()
},
Some(Box::new(PfmPfmContactManifoldGeneratorWorkspace::default())),
Some(ContactGeneratorWorkspace::from(
PfmPfmContactManifoldGeneratorWorkspace::default(),
)),
),
_ => (PrimitiveContactGenerator::default(), None),
}
@@ -116,21 +112,25 @@ impl ContactDispatcher for DefaultContactDispatcher {
&self,
shape1: ShapeType,
shape2: ShapeType,
) -> (ContactPhase, Option<Box<dyn Any + Send + Sync>>) {
) -> (ContactPhase, Option<ContactGeneratorWorkspace>) {
match (shape1, shape2) {
(ShapeType::Trimesh, _) | (_, ShapeType::Trimesh) => (
ContactPhase::NearPhase(ContactGenerator {
generate_contacts: super::generate_contacts_trimesh_shape,
..ContactGenerator::default()
}),
Some(Box::new(TrimeshShapeContactGeneratorWorkspace::new())),
Some(ContactGeneratorWorkspace::from(
TrimeshShapeContactGeneratorWorkspace::new(),
)),
),
(ShapeType::HeightField, _) | (_, ShapeType::HeightField) => (
ContactPhase::NearPhase(ContactGenerator {
generate_contacts: super::generate_contacts_heightfield_shape,
..ContactGenerator::default()
}),
Some(Box::new(HeightFieldShapeContactGeneratorWorkspace::new())),
Some(ContactGeneratorWorkspace::from(
HeightFieldShapeContactGeneratorWorkspace::new(),
)),
),
_ => {
let (gen, workspace) = self.dispatch_primitives(shape1, shape2);

8
src/geometry/contact_generator/contact_generator.rs
View File

@@ -1,3 +1,4 @@
use crate::data::MaybeSerializableData;
use crate::geometry::{
Collider, ColliderSet, ContactDispatcher, ContactEvent, ContactManifold, ContactPair, Shape,
SolverFlags,
@@ -6,7 +7,6 @@ use crate::math::Isometry;
#[cfg(feature = "simd-is-enabled")]
use crate::math::{SimdFloat, SIMD_WIDTH};
use crate::pipeline::EventHandler;
use std::any::Any;
#[derive(Copy, Clone)]
pub enum ContactPhase {
@@ -84,7 +84,7 @@ impl ContactPhase {
let mut manifold_arr: ArrayVec<[&mut ContactManifold; SIMD_WIDTH]> =
ArrayVec::new();
let mut workspace_arr: ArrayVec<
[Option<&mut (dyn Any + Send + Sync)>; SIMD_WIDTH],
[Option<&mut (dyn MaybeSerializableData)>; SIMD_WIDTH],
> = ArrayVec::new();
for (pair, solver_flags) in
@@ -148,7 +148,7 @@ pub struct PrimitiveContactGenerationContext<'a> {
pub position1: &'a Isometry<f32>,
pub position2: &'a Isometry<f32>,
pub manifold: &'a mut ContactManifold,
pub workspace: Option<&'a mut (dyn Any + Send + Sync)>,
pub workspace: Option<&'a mut (dyn MaybeSerializableData)>,
}
#[cfg(feature = "simd-is-enabled")]
@@ -161,7 +161,7 @@ pub struct PrimitiveContactGenerationContextSimd<'a, 'b> {
pub positions1: &'a Isometry<SimdFloat>,
pub positions2: &'a Isometry<SimdFloat>,
pub manifolds: &'a mut [&'b mut ContactManifold],
pub workspaces: &'a mut [Option<&'b mut (dyn Any + Send + Sync)>],
pub workspaces: &'a mut [Option<&'b mut (dyn MaybeSerializableData)>],
}
#[derive(Copy, Clone)]

98
src/geometry/contact_generator/contact_generator_workspace.rs Normal file
View File

@@ -0,0 +1,98 @@
use crate::data::MaybeSerializableData;
#[cfg(feature = "dim3")]
use crate::geometry::contact_generator::PfmPfmContactManifoldGeneratorWorkspace;
use crate::geometry::contact_generator::{
HeightFieldShapeContactGeneratorWorkspace, TrimeshShapeContactGeneratorWorkspace,
WorkspaceSerializationTag,
};
// Note we have this newtype because it simplifies the serialization/deserialization code.
pub struct ContactGeneratorWorkspace(pub Box<dyn MaybeSerializableData>);
impl<T: MaybeSerializableData> From<T> for ContactGeneratorWorkspace {
fn from(data: T) -> Self {
Self(Box::new(data) as Box<dyn MaybeSerializableData>)
}
}
#[cfg(feature = "serde-serialize")]
impl serde::Serialize for ContactGeneratorWorkspace {
fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
where
S: serde::Serializer,
{
use crate::serde::ser::SerializeStruct;
if let Some((tag, ser)) = self.0.as_serialize() {
let mut state = serializer.serialize_struct("ContactGeneratorWorkspace", 2)?;
state.serialize_field("tag", &tag)?;
state.serialize_field("inner", ser)?;
state.end()
} else {
Err(serde::ser::Error::custom(
"Found a non-serializable contact generator workspace.",
))
}
}
}
#[cfg(feature = "serde-serialize")]
impl<'de> serde::Deserialize<'de> for ContactGeneratorWorkspace {
fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
where
D: serde::Deserializer<'de>,
{
struct Visitor {};
impl<'de> serde::de::Visitor<'de> for Visitor {
type Value = ContactGeneratorWorkspace;
fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result {
write!(formatter, "one shape type tag and the inner shape data")
}
fn visit_seq<A>(self, mut seq: A) -> Result<Self::Value, A::Error>
where
A: serde::de::SeqAccess<'de>,
{
use num::cast::FromPrimitive;
let tag: u32 = seq
.next_element()?
.ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
fn deser<'de, A, S: MaybeSerializableData + serde::Deserialize<'de>>(
seq: &mut A,
) -> Result<Box<dyn MaybeSerializableData>, A::Error>
where
A: serde::de::SeqAccess<'de>,
{
let workspace: S = seq.next_element()?.ok_or_else(|| {
serde::de::Error::custom("Failed to deserialize builtin workspace.")
})?;
Ok(Box::new(workspace) as Box<dyn MaybeSerializableData>)
}
let workspace = match WorkspaceSerializationTag::from_u32(tag) {
Some(WorkspaceSerializationTag::HeightfieldShapeContactGeneratorWorkspace) => {
deser::<A, HeightFieldShapeContactGeneratorWorkspace>(&mut seq)?
}
Some(WorkspaceSerializationTag::TrimeshShapeContactGeneratorWorkspace) => {
deser::<A, TrimeshShapeContactGeneratorWorkspace>(&mut seq)?
}
#[cfg(feature = "dim3")]
Some(WorkspaceSerializationTag::PfmPfmContactGeneratorWorkspace) => {
deser::<A, PfmPfmContactManifoldGeneratorWorkspace>(&mut seq)?
}
None => {
return Err(serde::de::Error::custom(
"found invalid contact generator workspace type to deserialize",
))
}
};
Ok(ContactGeneratorWorkspace(workspace))
}
}
deserializer.deserialize_struct("ContactGeneratorWorkspace", &["tag", "inner"], Visitor {})
}
}

80
src/geometry/contact_generator/heightfield_shape_contact_generator.rs
View File

@@ -1,23 +1,29 @@
use crate::data::hashmap::{Entry, HashMap};
use crate::data::MaybeSerializableData;
use crate::geometry::contact_generator::{
ContactGenerationContext, PrimitiveContactGenerationContext, PrimitiveContactGenerator,
ContactGenerationContext, ContactGeneratorWorkspace, PrimitiveContactGenerationContext,
PrimitiveContactGenerator,
};
#[cfg(feature = "dim2")]
use crate::geometry::Capsule;
use crate::geometry::{Collider, ContactManifold, HeightField, Shape, ShapeType};
use crate::geometry::{Collider, ContactManifold, HeightField, Shape};
use crate::ncollide::bounding_volume::BoundingVolume;
use std::any::Any;
use std::collections::hash_map::Entry;
use std::collections::HashMap;
#[cfg(feature = "serde-serialize")]
use erased_serde::Serialize;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
struct SubDetector {
generator: PrimitiveContactGenerator,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
generator: Option<PrimitiveContactGenerator>,
manifold_id: usize,
timestamp: bool,
workspace: Option<Box<(dyn Any + Send + Sync)>>,
workspace: Option<ContactGeneratorWorkspace>,
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct HeightFieldShapeContactGeneratorWorkspace {
timestamp: bool,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
old_manifolds: Vec<ContactManifold>,
sub_detectors: HashMap<usize, SubDetector>,
}
@@ -55,36 +61,9 @@ fn do_generate_contacts(
.generator_workspace
.as_mut()
.expect("The HeightFieldShapeContactGeneratorWorkspace is missing.")
.0
.downcast_mut()
.expect("Invalid workspace type, expected a HeightFieldShapeContactGeneratorWorkspace.");
let shape_type2 = collider2.shape().shape_type();
/*
* Detect if the detector context has been reset.
*/
if !ctxt.pair.manifolds.is_empty() && workspace.sub_detectors.is_empty() {
// Rebuild the subdetector hashmap.
for (manifold_id, manifold) in ctxt.pair.manifolds.iter().enumerate() {
let subshape_id = if manifold.pair.collider1 == ctxt.pair.pair.collider1 {
manifold.subshape_index_pair.0
} else {
manifold.subshape_index_pair.1
};
let (generator, workspace2) = ctxt
.dispatcher
.dispatch_primitives(ShapeType::Capsule, shape_type2);
let sub_detector = SubDetector {
generator,
manifold_id,
timestamp: workspace.timestamp,
workspace: workspace2,
};
workspace.sub_detectors.insert(subshape_id, sub_detector);
}
}
let new_timestamp = !workspace.timestamp;
workspace.timestamp = new_timestamp;
@@ -127,7 +106,7 @@ fn do_generate_contacts(
let (generator, workspace2) =
dispatcher.dispatch_primitives(sub_shape1.shape_type(), shape_type2);
let sub_detector = SubDetector {
generator,
generator: Some(generator),
manifold_id: manifolds.len(),
timestamp: new_timestamp,
workspace: workspace2,
@@ -146,6 +125,19 @@ fn do_generate_contacts(
}
};
if sub_detector.generator.is_none() {
// We probably lost the generator after deserialization.
// So we need to dispatch again.
let (generator, workspace2) =
dispatcher.dispatch_primitives(sub_shape1.shape_type(), shape_type2);
sub_detector.generator = Some(generator);
// Don't overwrite the workspace if we already deserialized one.
if sub_detector.workspace.is_none() {
sub_detector.workspace = workspace2;
}
}
let manifold = &mut manifolds[sub_detector.manifold_id];
let mut ctxt2 = if coll_pair.collider1 != manifold.pair.collider1 {
@@ -158,7 +150,7 @@ fn do_generate_contacts(
position1: collider2.position(),
position2: position1,
manifold,
workspace: sub_detector.workspace.as_deref_mut(),
workspace: sub_detector.workspace.as_mut().map(|w| &mut *w.0),
}
} else {
PrimitiveContactGenerationContext {
@@ -170,14 +162,24 @@ fn do_generate_contacts(
position1,
position2: collider2.position(),
manifold,
workspace: sub_detector.workspace.as_deref_mut(),
workspace: sub_detector.workspace.as_mut().map(|w| &mut *w.0),
}
};
(sub_detector.generator.generate_contacts)(&mut ctxt2)
(sub_detector.generator.unwrap().generate_contacts)(&mut ctxt2)
});
workspace
.sub_detectors
.retain(|_, detector| detector.timestamp == new_timestamp)
}
impl MaybeSerializableData for HeightFieldShapeContactGeneratorWorkspace {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> {
Some((
super::WorkspaceSerializationTag::HeightfieldShapeContactGeneratorWorkspace as u32,
self,
))
}
}

5
src/geometry/contact_generator/mod.rs
View File

@@ -23,6 +23,7 @@ pub use self::pfm_pfm_contact_generator::{
generate_contacts_pfm_pfm, PfmPfmContactManifoldGeneratorWorkspace,
};
// pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon;
pub use self::contact_generator_workspace::ContactGeneratorWorkspace;
pub use self::trimesh_shape_contact_generator::{
generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace,
};
@@ -31,12 +32,15 @@ pub(crate) use self::polygon_polygon_contact_generator::clip_segments;
#[cfg(feature = "dim2")]
pub(crate) use self::polygon_polygon_contact_generator::clip_segments_with_normal;
pub(self) use self::serializable_workspace_tag::WorkspaceSerializationTag;
mod ball_ball_contact_generator;
mod ball_convex_contact_generator;
mod ball_polygon_contact_generator;
mod capsule_capsule_contact_generator;
mod contact_dispatcher;
mod contact_generator;
mod contact_generator_workspace;
mod cuboid_capsule_contact_generator;
mod cuboid_cuboid_contact_generator;
mod cuboid_polygon_contact_generator;
@@ -45,6 +49,7 @@ mod heightfield_shape_contact_generator;
#[cfg(feature = "dim3")]
mod pfm_pfm_contact_generator;
mod polygon_polygon_contact_generator;
mod serializable_workspace_tag;
mod trimesh_shape_contact_generator;
use crate::geometry::{Contact, ContactManifold};

20
src/geometry/contact_generator/pfm_pfm_contact_generator.rs
View File

@@ -1,14 +1,24 @@
use crate::data::MaybeSerializableData;
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{KinematicsCategory, PolygonalFeatureMap, PolyhedronFace};
use crate::math::{Isometry, Vector};
#[cfg(feature = "serde-serialize")]
use erased_serde::Serialize;
use na::Unit;
use ncollide::query;
use ncollide::query::algorithms::{gjk::GJKResult, VoronoiSimplex};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct PfmPfmContactManifoldGeneratorWorkspace {
#[cfg_attr(
feature = "serde-serialize",
serde(skip, default = "VoronoiSimplex::new")
)]
simplex: VoronoiSimplex<f32>,
last_gjk_dir: Option<Unit<Vector<f32>>>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
feature1: PolyhedronFace,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
feature2: PolyhedronFace,
}
@@ -117,3 +127,13 @@ fn do_generate_contacts(
// Transfer impulses.
super::match_contacts(&mut ctxt.manifold, &old_manifold_points, false);
}
impl MaybeSerializableData for PfmPfmContactManifoldGeneratorWorkspace {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> {
Some((
super::WorkspaceSerializationTag::PfmPfmContactGeneratorWorkspace as u32,
self,
))
}
}

9
src/geometry/contact_generator/serializable_workspace_tag.rs Normal file
View File

@@ -0,0 +1,9 @@
use num_derive::FromPrimitive;
#[derive(Copy, Clone, Debug, FromPrimitive)]
pub(super) enum WorkspaceSerializationTag {
TrimeshShapeContactGeneratorWorkspace = 0,
#[cfg(feature = "dim3")]
PfmPfmContactGeneratorWorkspace,
HeightfieldShapeContactGeneratorWorkspace,
}

25
src/geometry/contact_generator/trimesh_shape_contact_generator.rs
View File

@@ -1,13 +1,18 @@
use crate::data::MaybeSerializableData;
use crate::geometry::contact_generator::{
ContactGenerationContext, PrimitiveContactGenerationContext,
};
use crate::geometry::{Collider, ContactManifold, ShapeType, Trimesh};
use crate::ncollide::bounding_volume::{BoundingVolume, AABB};
#[cfg(feature = "serde-serialize")]
use erased_serde::Serialize;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct TrimeshShapeContactGeneratorWorkspace {
interferences: Vec<usize>,
local_aabb2: AABB<f32>,
old_interferences: Vec<usize>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
old_manifolds: Vec<ContactManifold>,
}
@@ -51,6 +56,7 @@ fn do_generate_contacts(
.generator_workspace
.as_mut()
.expect("The TrimeshShapeContactGeneratorWorkspace is missing.")
.0
.downcast_mut()
.expect("Invalid workspace type, expected a TrimeshShapeContactGeneratorWorkspace.");
@@ -83,7 +89,7 @@ fn do_generate_contacts(
// This happens if for some reasons the contact generator context was lost
// and rebuilt. In this case, we hate to reconstruct the `old_interferences`
// array using the subshape ids from the contact manifolds.
// TODO: always rely on the subshape ids instead of maintaining `.ord_interferences` ?
// TODO: always rely on the subshape ids instead of maintaining `.old_interferences` ?
let ctxt_collider1 = ctxt_pair_pair.collider1;
workspace.old_interferences = workspace
.old_manifolds
@@ -97,6 +103,7 @@ fn do_generate_contacts(
})
.collect();
}
// This assertion may fire due to the invalid triangle_ids that the
// near-phase may return (due to SIMD sentinels).
//
@@ -123,6 +130,8 @@ fn do_generate_contacts(
let mut old_manifolds_it = workspace.old_manifolds.drain(..);
let shape_type2 = collider2.shape().shape_type();
// TODO: don't redispatch at each frame (we should probably do the same as
// the heightfield).
for (i, triangle_id) in new_interferences.iter().enumerate() {
if *triangle_id >= trimesh1.num_triangles() {
// Because of SIMD padding, the broad-phase may return tiangle indices greater
@@ -176,7 +185,7 @@ fn do_generate_contacts(
position1: collider2.position(),
position2: collider1.position(),
manifold,
workspace: workspace2.as_deref_mut(),
workspace: workspace2.as_mut().map(|w| &mut *w.0),
}
} else {
PrimitiveContactGenerationContext {
@@ -188,10 +197,20 @@ fn do_generate_contacts(
position1: collider1.position(),
position2: collider2.position(),
manifold,
workspace: workspace2.as_deref_mut(),
workspace: workspace2.as_mut().map(|w| &mut *w.0),
}
};
(generator.generate_contacts)(&mut ctxt2);
}
}
impl MaybeSerializableData for TrimeshShapeContactGeneratorWorkspace {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> {
Some((
super::WorkspaceSerializationTag::TrimeshShapeContactGeneratorWorkspace as u32,
self,
))
}
}

6
src/geometry/narrow_phase.rs
View File

@@ -424,7 +424,11 @@ impl NarrowPhase {
let (generator, workspace) =
dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type());
pair.generator = Some(generator);
pair.generator_workspace = workspace;
// Keep the workspace if one already exists.
if pair.generator_workspace.is_none() {
pair.generator_workspace = workspace;
}
}
let context = ContactGenerationContext {

12
src/geometry/polyhedron_feature3d.rs
View File

@@ -14,6 +14,18 @@ pub struct PolyhedronFace {
pub num_vertices: usize,
}
impl Default for PolyhedronFace {
fn default() -> Self {
Self {
vertices: [Point::origin(); 4],
vids: [0; 4],
eids: [0; 4],
fid: 0,
num_vertices: 0,
}
}
}
impl From<CuboidFeatureFace> for PolyhedronFace {
fn from(face: CuboidFeatureFace) -> Self {
Self {

128
src/utils.rs
View File

@@ -1,13 +1,10 @@
//! Miscellaneous utilities.
use crate::dynamics::RigidBodyHandle;
#[cfg(all(feature = "enhanced-determinism", feature = "serde-serialize"))]
use indexmap::IndexMap as HashMap;
use na::{Matrix2, Matrix3, Matrix3x2, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3};
use num::Zero;
use simba::simd::SimdValue;
#[cfg(all(not(feature = "enhanced-determinism"), feature = "serde-serialize"))]
use std::collections::HashMap;
use std::ops::{Add, Mul};
use {
crate::simd::{SimdBool, SimdFloat},
@@ -1200,129 +1197,6 @@ impl Drop for FlushToZeroDenormalsAreZeroFlags {
}
}
#[cfg(feature = "serde-serialize")]
pub(crate) fn serialize_hashmap_capacity<S: serde::Serializer, K, V, H: std::hash::BuildHasher>(
map: &HashMap<K, V, H>,
s: S,
) -> Result<S::Ok, S::Error> {
s.serialize_u64(map.capacity() as u64)
}
#[cfg(feature = "serde-serialize")]
pub(crate) fn deserialize_hashmap_capacity<
'de,
D: serde::Deserializer<'de>,
K,
V,
H: std::hash::BuildHasher + Default,
>(
d: D,
) -> Result<HashMap<K, V, H>, D::Error> {
struct CapacityVisitor;
impl<'de> serde::de::Visitor<'de> for CapacityVisitor {
type Value = u64;
fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result {
write!(formatter, "an integer between 0 and 2^64")
}
fn visit_u64<E: serde::de::Error>(self, val: u64) -> Result<Self::Value, E> {
Ok(val)
}
}
let capacity = d.deserialize_u64(CapacityVisitor)? as usize;
Ok(HashMap::with_capacity_and_hasher(
capacity,
Default::default(),
))
}
/*
* FxHasher taken from rustc_hash, except that it does not depend on the pointer size.
*/
#[cfg(feature = "enhanced-determinism")]
pub(crate) type FxHashMap32<K, V> =
indexmap::IndexMap<K, V, std::hash::BuildHasherDefault<FxHasher32>>;
const K: u32 = 0x9e3779b9;
pub(crate) struct FxHasher32 {
hash: u32,
}
impl Default for FxHasher32 {
#[inline]
fn default() -> FxHasher32 {
FxHasher32 { hash: 0 }
}
}
impl FxHasher32 {
#[inline]
fn add_to_hash(&mut self, i: u32) {
use std::ops::BitXor;
self.hash = self.hash.rotate_left(5).bitxor(i).wrapping_mul(K);
}
}
impl std::hash::Hasher for FxHasher32 {
#[inline]
fn write(&mut self, mut bytes: &[u8]) {
use std::convert::TryInto;
let read_u32 = |bytes: &[u8]| u32::from_ne_bytes(bytes[..4].try_into().unwrap());
let mut hash = FxHasher32 { hash: self.hash };
assert!(std::mem::size_of::<u32>() <= 8);
while bytes.len() >= std::mem::size_of::<u32>() {
hash.add_to_hash(read_u32(bytes) as u32);
bytes = &bytes[std::mem::size_of::<u32>()..];
}
if (std::mem::size_of::<u32>() > 4) && (bytes.len() >= 4) {
hash.add_to_hash(u32::from_ne_bytes(bytes[..4].try_into().unwrap()) as u32);
bytes = &bytes[4..];
}
if (std::mem::size_of::<u32>() > 2) && bytes.len() >= 2 {
hash.add_to_hash(u16::from_ne_bytes(bytes[..2].try_into().unwrap()) as u32);
bytes = &bytes[2..];
}
if (std::mem::size_of::<u32>() > 1) && bytes.len() >= 1 {
hash.add_to_hash(bytes[0] as u32);
}
self.hash = hash.hash;
}
#[inline]
fn write_u8(&mut self, i: u8) {
self.add_to_hash(i as u32);
}
#[inline]
fn write_u16(&mut self, i: u16) {
self.add_to_hash(i as u32);
}
#[inline]
fn write_u32(&mut self, i: u32) {
self.add_to_hash(i as u32);
}
#[inline]
fn write_u64(&mut self, i: u64) {
self.add_to_hash(i as u32);
self.add_to_hash((i >> 32) as u32);
}
#[inline]
fn write_usize(&mut self, i: usize) {
self.add_to_hash(i as u32);
}
#[inline]
fn finish(&self) -> u64 {
self.hash as u64
}
}
pub(crate) fn other_handle(
pair: (RigidBodyHandle, RigidBodyHandle),
handle: RigidBodyHandle,