Fix simulation reaching different states when started from different snaphots.

12 files changed, 236 insertions, 76 deletions

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<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),
             )
         }
     }
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<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);
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<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)]
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<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)?
+                    }
+                    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 {})
+    }
+}
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<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 +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<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,
+        ))
+    }
+}
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<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,
+        ))
+    }
+}
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<CuboidFeatureFace> for PolyhedronFace {
     fn from(face: CuboidFeatureFace) -> Self {
         Self {