diff options
Diffstat (limited to 'src')
65 files changed, 2730 insertions, 916 deletions
diff --git a/src/data/graph.rs b/src/data/graph.rs index ea27e03..de958c3 100644 --- a/src/data/graph.rs +++ b/src/data/graph.rs @@ -749,20 +749,12 @@ impl<N, E> IndexMut<EdgeIndex> for Graph<N, E> { /// The walker does not borrow from the graph, so it lets you step through /// neighbors or incident edges while also mutating graph weights, as /// in the following example: +#[derive(Clone)] pub struct WalkNeighbors { skip_start: NodeIndex, next: [EdgeIndex; 2], } -impl Clone for WalkNeighbors { - fn clone(&self) -> Self { - WalkNeighbors { - skip_start: self.skip_start, - next: self.next, - } - } -} - /// Reference to a `Graph` edge. #[derive(Debug)] pub struct EdgeReference<'a, E: 'a> { diff --git a/src/data/hashmap.rs b/src/data/hashmap.rs new file mode 100644 index 0000000..d2ea980 --- /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<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 + } +} diff --git a/src/data/maybe_serializable_data.rs b/src/data/maybe_serializable_data.rs new file mode 100644 index 0000000..8b14e1a --- /dev/null +++ b/src/data/maybe_serializable_data.rs @@ -0,0 +1,17 @@ +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 + } + + /// Clones `self`. + fn clone_dyn(&self) -> Box<dyn MaybeSerializableData>; +} + +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/data/pubsub.rs b/src/data/pubsub.rs index b2c9e27..80fb3a2 100644 --- a/src/data/pubsub.rs +++ b/src/data/pubsub.rs @@ -5,6 +5,7 @@ use std::marker::PhantomData; /// A permanent subscription to a pub-sub queue. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] pub struct Subscription<T> { // Position on the cursor array. id: u32, @@ -12,6 +13,7 @@ pub struct Subscription<T> { } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] struct PubSubCursor { // Position on the offset array. id: u32, @@ -36,6 +38,7 @@ impl PubSubCursor { /// A pub-sub queue. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] pub struct PubSub<T> { deleted_messages: u32, deleted_offsets: u32, diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs index 074f802..9fe6488 100644 --- a/src/dynamics/joint/joint.rs +++ b/src/dynamics/joint/joint.rs @@ -95,6 +95,7 @@ impl From<PrismaticJoint> for JointParams { } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] /// A joint attached to two bodies. pub struct Joint { /// Handle to the first body attached to this joint. 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<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, diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs index 22e7da5..73e9b0d 100644 --- a/src/dynamics/mass_properties.rs +++ b/src/dynamics/mass_properties.rs @@ -25,8 +25,11 @@ pub struct MassProperties { } impl MassProperties { + /// Initializes the mass properties with the given center-of-mass, mass, and angular inertia. + /// + /// The center-of-mass is specified in the local-space of the rigid-body. #[cfg(feature = "dim2")] - pub(crate) fn new(local_com: Point<f32>, mass: f32, principal_inertia: f32) -> Self { + pub fn new(local_com: Point<f32>, mass: f32, principal_inertia: f32) -> Self { let inv_mass = utils::inv(mass); let inv_principal_inertia_sqrt = utils::inv(principal_inertia.sqrt()); Self { @@ -36,13 +39,23 @@ impl MassProperties { } } + /// Initializes the mass properties from the given center-of-mass, mass, and principal angular inertia. + /// + /// The center-of-mass is specified in the local-space of the rigid-body. + /// The principal angular inertia are the angular inertia along the coordinate axes in the local-space + /// of the rigid-body. #[cfg(feature = "dim3")] - pub(crate) fn new(local_com: Point<f32>, mass: f32, principal_inertia: AngVector<f32>) -> Self { + pub fn new(local_com: Point<f32>, mass: f32, principal_inertia: AngVector<f32>) -> Self { Self::with_principal_inertia_frame(local_com, mass, principal_inertia, Rotation::identity()) } + /// Initializes the mass properties from the given center-of-mass, mass, and principal angular inertia. + /// + /// The center-of-mass is specified in the local-space of the rigid-body. + /// The principal angular inertia are the angular inertia along the coordinate axes defined by + /// the `principal_inertia_local_frame` expressed in the local-space of the rigid-body. #[cfg(feature = "dim3")] - pub(crate) fn with_principal_inertia_frame( + pub fn with_principal_inertia_frame( local_com: Point<f32>, mass: f32, principal_inertia: AngVector<f32>, @@ -91,7 +104,7 @@ impl MassProperties { } #[cfg(feature = "dim3")] - /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axii. + /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axes. pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3<f32> { let inv_principal_inertia = self.inv_principal_inertia_sqrt.map(|e| e * e); self.principal_inertia_local_frame.to_rotation_matrix() @@ -103,7 +116,7 @@ impl MassProperties { } #[cfg(feature = "dim3")] - /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii. + /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axes. pub fn reconstruct_inertia_matrix(&self) -> Matrix3<f32> { let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e)); self.principal_inertia_local_frame.to_rotation_matrix() diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs index 5f08958..3b1b214 100644 --- a/src/dynamics/mass_properties_capsule.rs +++ b/src/dynamics/mass_properties_capsule.rs @@ -1,30 +1,9 @@ use crate::dynamics::MassProperties; #[cfg(feature = "dim3")] use crate::geometry::Capsule; -use crate::math::{Point, PrincipalAngularInertia, Vector}; +use crate::math::Point; impl MassProperties { - fn cylinder_y_volume_unit_inertia( - half_height: f32, - radius: f32, - ) -> (f32, PrincipalAngularInertia<f32>) { - #[cfg(feature = "dim2")] - { - Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height)) - } - - #[cfg(feature = "dim3")] - { - let volume = half_height * radius * radius * std::f32::consts::PI * 2.0; - let sq_radius = radius * radius; - let sq_height = half_height * half_height * 4.0; - let off_principal = (sq_radius * 3.0 + sq_height) / 12.0; - - let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal); - (volume, inertia) - } - } - pub(crate) fn from_capsule(density: f32, a: Point<f32>, b: Point<f32>, radius: f32) -> Self { let half_height = (b - a).norm() / 2.0; let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); diff --git a/src/dynamics/mass_properties_cone.rs b/src/dynamics/mass_properties_cone.rs new file mode 100644 index 0000000..12f831f --- /dev/null +++ b/src/dynamics/mass_properties_cone.rs @@ -0,0 +1,29 @@ +use crate::dynamics::MassProperties; +use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector}; + +impl MassProperties { + pub(crate) fn cone_y_volume_unit_inertia( + half_height: f32, + radius: f32, + ) -> (f32, PrincipalAngularInertia<f32>) { + let volume = radius * radius * std::f32::consts::PI * half_height * 2.0 / 3.0; + let sq_radius = radius * radius; + let sq_height = half_height * half_height * 4.0; + let off_principal = sq_radius * 3.0 / 20.0 + sq_height * 3.0 / 5.0; + let principal = sq_radius * 3.0 / 10.0; + + (volume, Vector::new(off_principal, principal, off_principal)) + } + + pub(crate) fn from_cone(density: f32 |