From 01af6c09a786ec2d6475531a7dac873eeffa3a3e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 2 Nov 2020 15:07:50 +0100 Subject: Fix simulation reaching different states when started from different snaphots. --- src/data/hashmap.rs | 137 +++++++++++++++++++++ src/data/maybe_serializable_data.rs | 14 +++ src/data/mod.rs | 4 + src/geometry/broad_phase_multi_sap.rs | 9 +- src/geometry/contact.rs | 15 ++- .../contact_generator/contact_dispatcher.rs | 32 ++--- .../contact_generator/contact_generator.rs | 6 +- .../contact_generator_workspace.rs | 95 ++++++++++++++ .../heightfield_shape_contact_generator.rs | 79 ++++++------ src/geometry/contact_generator/mod.rs | 5 + .../contact_generator/pfm_pfm_contact_generator.rs | 20 +++ .../serializable_workspace_tag.rs | 8 ++ .../trimesh_shape_contact_generator.rs | 25 +++- src/geometry/narrow_phase.rs | 6 +- src/geometry/polyhedron_feature3d.rs | 12 ++ src/utils.rs | 128 +------------------ 16 files changed, 392 insertions(+), 203 deletions(-) create mode 100644 src/data/hashmap.rs create mode 100644 src/data/maybe_serializable_data.rs create mode 100644 src/geometry/contact_generator/contact_generator_workspace.rs create mode 100644 src/geometry/contact_generator/serializable_workspace_tag.rs (limited to 'src') diff --git a/src/data/hashmap.rs b/src/data/hashmap.rs new file mode 100644 index 0000000..78f9797 --- /dev/null +++ b/src/data/hashmap.rs @@ -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( + map: &StdHashMap, + s: S, +) -> Result { + 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, 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(self, val: u64) -> Result { + 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 = indexmap::IndexMap>; +#[cfg(not(feature = "enhanced-determinism"))] +pub use rustc_hash::{Entry, FxHashMap as HashMap}; +#[cfg(feature = "enhanced-determinism")] +pub use {self::FxHashMap32 as HashMap, indexmap::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::() <= 8); + while bytes.len() >= std::mem::size_of::() { + hash.add_to_hash(read_u32(bytes) as u32); + bytes = &bytes[std::mem::size_of::()..]; + } + if (std::mem::size_of::() > 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::() > 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::() > 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 + } +} diff --git a/src/data/maybe_serializable_data.rs b/src/data/maybe_serializable_data.rs new file mode 100644 index 0000000..db38963 --- /dev/null +++ b/src/data/maybe_serializable_data.rs @@ -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); diff --git a/src/data/mod.rs b/src/data/mod.rs index 5d3efa6..0e0459e 100644 --- a/src/data/mod.rs +++ b/src/data/mod.rs @@ -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; diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 3562c2e..1afda5a 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -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}; @@ -433,8 +430,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 diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index d8f3632..53e9064 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact.rs @@ -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, #[cfg_attr(feature = "serde-serialize", serde(skip))] pub(crate) generator: Option, - #[cfg_attr(feature = "serde-serialize", serde(skip))] - pub(crate) generator_workspace: Option>, + pub(crate) generator_workspace: Option, } impl ContactPair { pub(crate) fn new( pair: ColliderPair, generator: ContactPhase, - generator_workspace: Option>, + generator_workspace: Option, ) -> 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), ) } } diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs index 1872c7b..7c32714 100644 --- a/src/geometry/contact_generator/contact_dispatcher.rs +++ b/src/geometry/contact_generator/contact_dispatcher.rs @@ -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>, - ); + ) -> (PrimitiveContactGenerator, Option); /// Select the collision-detection algorithm for the given pair of non-primitive shapes. fn dispatch( &self, shape1: ShapeType, shape2: ShapeType, - ) -> (ContactPhase, Option>); + ) -> (ContactPhase, Option); } /// The default contact dispatcher used by Rapier. @@ -35,10 +32,7 @@ impl ContactDispatcher for DefaultContactDispatcher { &self, shape1: ShapeType, shape2: ShapeType, - ) -> ( - PrimitiveContactGenerator, - Option>, - ) { + ) -> (PrimitiveContactGenerator, Option) { 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>) { + ) -> (ContactPhase, Option) { 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); diff --git a/src/geometry/contact_generator/contact_generator.rs b/src/geometry/contact_generator/contact_generator.rs index 728794d..de35b92 100644 --- a/src/geometry/contact_generator/contact_generator.rs +++ b/src/geometry/contact_generator/contact_generator.rs @@ -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 { @@ -148,7 +148,7 @@ pub struct PrimitiveContactGenerationContext<'a> { pub position1: &'a Isometry, pub position2: &'a Isometry, 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, pub positions2: &'a Isometry, 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)] diff --git a/src/geometry/contact_generator/contact_generator_workspace.rs b/src/geometry/contact_generator/contact_generator_workspace.rs new file mode 100644 index 0000000..ece0a6a --- /dev/null +++ b/src/geometry/contact_generator/contact_generator_workspace.rs @@ -0,0 +1,95 @@ +use crate::data::MaybeSerializableData; +use crate::geometry::contact_generator::{ + HeightFieldShapeContactGeneratorWorkspace, PfmPfmContactManifoldGeneratorWorkspace, + TrimeshShapeContactGeneratorWorkspace, WorkspaceSerializationTag, +}; + +// Note we have this newtype because it simplifies the serialization/deserialization code. +pub struct ContactGeneratorWorkspace(pub Box); + +impl From for ContactGeneratorWorkspace { + fn from(data: T) -> Self { + Self(Box::new(data) as Box) + } +} + +#[cfg(feature = "serde-serialize")] +impl serde::Serialize for ContactGeneratorWorkspace { + fn serialize(&self, serializer: S) -> Result + 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(deserializer: D) -> Result + 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(self, mut seq: A) -> Result + 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, 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) + } + + let workspace = match WorkspaceSerializationTag::from_u32(tag) { + Some(WorkspaceSerializationTag::HeightfieldShapeContactGeneratorWorkspace) => { + deser::(&mut seq)? + } + Some(WorkspaceSerializationTag::TrimeshShapeContactGeneratorWorkspace) => { + deser::(&mut seq)? + } + Some(WorkspaceSerializationTag::PfmPfmContactGeneratorWorkspace) => { + deser::(&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 {}) + } +} diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index f291fa0..43cf17b 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -1,23 +1,28 @@ +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; +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, manifold_id: usize, timestamp: bool, - workspace: Option>, + workspace: Option, } +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct HeightFieldShapeContactGeneratorWorkspace { timestamp: bool, + #[cfg_attr(feature = "serde-serialize", serde(skip))] old_manifolds: Vec, sub_detectors: HashMap, } @@ -55,36 +60,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 +105,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 +124,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 +149,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 +161,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, + )) + } +} diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs index 0549420..9b14711 100644 --- a/src/geometry/contact_generator/mod.rs +++ b/src/geometry/contact_generator/mod.rs @@ -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}; diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs index 1dcae33..027c398 100644 --- a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs +++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs @@ -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, last_gjk_dir: Option>>, + #[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, + )) + } +} diff --git a/src/geometry/contact_generator/serializable_workspace_tag.rs b/src/geometry/contact_generator/serializable_workspace_tag.rs new file mode 100644 index 0000000..adedb2d --- /dev/null +++ b/src/geometry/contact_generator/serializable_workspace_tag.rs @@ -0,0 +1,8 @@ +use num_derive::FromPrimitive; + +#[derive(Copy, Clone, Debug, FromPrimitive)] +pub(super) enum WorkspaceSerializationTag { + TrimeshShapeContactGeneratorWorkspace = 0, + PfmPfmContactGeneratorWorkspace, + HeightfieldShapeContactGeneratorWorkspace, +} diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs index 9474516..26ccfaa 100644 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -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, local_aabb2: AABB, old_interferences: Vec, + #[cfg_attr(feature = "serde-serialize", serde(skip))] old_manifolds: Vec, } @@ -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, + )) + } +} diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index c1bd411..e3e52e5 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -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 { diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs index e88674e..d7f0a23 100644 --- a/src/geometry/polyhedron_feature3d.rs +++ b/src/geometry/polyhedron_feature3d.rs @@ -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 for PolyhedronFace { fn from(face: CuboidFeatureFace) -> Self { Self { diff --git a/src/utils.rs b/src/utils.rs index a398a02..6557b74 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -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( - map: &HashMap, - s: S, -) -> Result { - 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, 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(self, val: u64) -> Result { - 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 = - indexmap::IndexMap>; - -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::() <= 8); - while bytes.len() >= std::mem::size_of::() { - hash.add_to_hash(read_u32(bytes) as u32); - bytes = &bytes[std::mem::size_of::()..]; - } - if (std::mem::size_of::() > 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::() > 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::() > 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, -- cgit From 83482602717d1a9aebb181fb82456a292801cbdc Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 2 Nov 2020 17:57:48 +0100 Subject: Fix 2D compilation. --- src/data/hashmap.rs | 4 ++-- src/geometry/contact_generator/contact_generator.rs | 2 +- src/geometry/contact_generator/contact_generator_workspace.rs | 7 +++++-- .../contact_generator/heightfield_shape_contact_generator.rs | 1 + src/geometry/contact_generator/serializable_workspace_tag.rs | 1 + 5 files changed, 10 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/data/hashmap.rs b/src/data/hashmap.rs index 78f9797..d2ea980 100644 --- a/src/data/hashmap.rs +++ b/src/data/hashmap.rs @@ -51,10 +51,10 @@ pub fn deserialize_hashmap_capacity< */ #[cfg(feature = "enhanced-determinism")] pub type FxHashMap32 = indexmap::IndexMap>; -#[cfg(not(feature = "enhanced-determinism"))] -pub use rustc_hash::{Entry, FxHashMap as HashMap}; #[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; diff --git a/src/geometry/contact_generator/contact_generator.rs b/src/geometry/contact_generator/contact_generator.rs index de35b92..b1b1be6 100644 --- a/src/geometry/contact_generator/contact_generator.rs +++ b/src/geometry/contact_generator/contact_generator.rs @@ -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 diff --git a/src/geometry/contact_generator/contact_generator_workspace.rs b/src/geometry/contact_generator/contact_generator_workspace.rs index ece0a6a..fb6d7b4 100644 --- a/src/geometry/contact_generator/contact_generator_workspace.rs +++ b/src/geometry/contact_generator/contact_generator_workspace.rs @@ -1,7 +1,9 @@ use crate::data::MaybeSerializableData; +#[cfg(feature = "dim3")] +use crate::geometry::contact_generator::PfmPfmContactManifoldGeneratorWorkspace; use crate::geometry::contact_generator::{ - HeightFieldShapeContactGeneratorWorkspace, PfmPfmContactManifoldGeneratorWorkspace, - TrimeshShapeContactGeneratorWorkspace, WorkspaceSerializationTag, + HeightFieldShapeContactGeneratorWorkspace, TrimeshShapeContactGeneratorWorkspace, + WorkspaceSerializationTag, }; // Note we have this newtype because it simplifies the serialization/deserialization code. @@ -76,6 +78,7 @@ impl<'de> serde::Deserialize<'de> for ContactGeneratorWorkspace { Some(WorkspaceSerializationTag::TrimeshShapeContactGeneratorWorkspace) => { deser::(&mut seq)? } + #[cfg(feature = "dim3")] Some(WorkspaceSerializationTag::PfmPfmContactGeneratorWorkspace) => { deser::(&mut seq)? } diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index 43cf17b..62c6365 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -8,6 +8,7 @@ use crate::geometry::contact_generator::{ use crate::geometry::Capsule; use crate::geometry::{Collider, ContactManifold, HeightField, Shape}; use crate::ncollide::bounding_volume::BoundingVolume; +#[cfg(feature = "serde-serialize")] use erased_serde::Serialize; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] diff --git a/src/geometry/contact_generator/serializable_workspace_tag.rs b/src/geometry/contact_generator/serializable_workspace_tag.rs index adedb2d..2488c1e 100644 --- a/src/geometry/contact_generator/serializable_workspace_tag.rs +++ b/src/geometry/contact_generator/serializable_workspace_tag.rs @@ -3,6 +3,7 @@ use num_derive::FromPrimitive; #[derive(Copy, Clone, Debug, FromPrimitive)] pub(super) enum WorkspaceSerializationTag { TrimeshShapeContactGeneratorWorkspace = 0, + #[cfg(feature = "dim3")] PfmPfmContactGeneratorWorkspace, HeightfieldShapeContactGeneratorWorkspace, } -- cgit From a4a4ddb5b4f03a6cf855ff4120803863e3c60a4f Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 2 Nov 2020 18:47:27 +0100 Subject: Implement joint removal. --- src/dynamics/joint/joint_set.rs | 32 ++++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) (limited to 'src') diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs index 2b1895f..5144d97 100644 --- a/src/dynamics/joint/joint_set.rs +++ b/src/dynamics/joint/joint_set.rs @@ -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 { + 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, -- cgit