Voxel fracture experimentsvoxels

7 files changed, 191 insertions, 3 deletions

diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index a91c4b9..676fe69 100644
--- a/src/geometry/mod.rs
+++ b/src/geometry/mod.rs
@@ -54,6 +54,7 @@ pub type PointProjection = parry::query::PointProjection;
 /// The the time of impact between two shapes.
 pub type TOI = parry::query::TOI;
 pub use parry::shape::SharedShape;
+pub use parry::shape::{OctantPattern, VoxelType, Voxels};
 
 bitflags::bitflags! {
     /// Flags providing more information regarding a collision event.
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index dacca9e..70fa30f 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -1013,6 +1013,7 @@ impl NarrowPhase {
         bodies: &RigidBodySet,
         out_contact_pairs: &mut Vec<TemporaryInteractionIndex>,
         out_manifolds: &mut Vec<&'a mut ContactManifold>,
+        out_pairs: &mut Vec<TemporaryInteractionIndex>,
         out: &mut Vec<Vec<ContactManifoldIndex>>,
     ) {
         for out_island in &mut out[..islands.num_islands()] {
diff --git a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs
index bb9fd78..423a309 100644
--- a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs
+++ b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs
@@ -402,6 +402,7 @@ impl DebugRenderPipeline {
                 let vtx = s.to_polyline(self.style.border_subdivisions);
                 backend.draw_line_strip(object, &vtx, pos, &Vector::repeat(1.0), color, true)
             }
+            TypedShape::Voxels(_) => todo!(),
             TypedShape::Custom(_) => {}
         }
     }
@@ -555,6 +556,9 @@ impl DebugRenderPipeline {
                 let (vtx, idx) = s.to_outline(self.style.border_subdivisions);
                 backend.draw_polyline(object, &vtx, &idx, pos, &Vector::repeat(1.0), color)
             }
+            TypedShape::Voxels(s) => s.iter_outline(|a, b| {
+                backend.draw_line(object, pos * a, pos * b, color);
+            }),
             TypedShape::Custom(_) => {}
         }
     }
diff --git a/src/pipeline/event_handler.rs b/src/pipeline/event_handler.rs
index e5270ad..6e3667d 100644
--- a/src/pipeline/event_handler.rs
+++ b/src/pipeline/event_handler.rs
@@ -1,8 +1,15 @@
 use crate::dynamics::RigidBodySet;
-use crate::geometry::{ColliderSet, CollisionEvent, ContactForceEvent, ContactPair};
+use crate::geometry::{
+    ColliderHandle, ColliderSet, CollisionEvent, ContactForceEvent, ContactPair,
+};
 use crate::math::Real;
 use crossbeam::channel::Sender;
 
+pub struct FractureEvent {
+    pub fractured_collider: ColliderHandle,
+    pub fragments: Vec<ColliderHandle>,
+}
+
 bitflags::bitflags! {
     #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
     /// Flags affecting the events generated for this collider.
@@ -13,6 +20,9 @@ bitflags::bitflags! {
         /// If set, Rapier will call `EventHandler::handle_contact_force_event`
         /// whenever relevant for this collider.
         const CONTACT_FORCE_EVENTS = 0b0010;
+        /// If set, Rapier will call `EventHandler::handle_fracture_event` whenever this colliders
+        /// is fractured.
+        const FRACTURE_EVENTS = 0b0010;
     }
 }
 
@@ -66,6 +76,13 @@ pub trait EventHandler: Send + Sync {
         contact_pair: &ContactPair,
         total_force_magnitude: Real,
     );
+
+    fn handle_fracture_event(
+        &self,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
+        event: FractureEvent,
+    );
 }
 
 impl EventHandler for () {
@@ -87,12 +104,21 @@ impl EventHandler for () {
         _total_force_magnitude: Real,
     ) {
     }
+
+    fn handle_fracture_event(
+        &self,
+        _bodies: &RigidBodySet,
+        _colliders: &ColliderSet,
+        _event: FractureEvent,
+    ) {
+    }
 }
 
 /// A collision event handler that collects events into a crossbeam channel.
 pub struct ChannelEventCollector {
     collision_event_sender: Sender<CollisionEvent>,
     contact_force_event_sender: Sender<ContactForceEvent>,
+    fracture_event_sender: Sender<FractureEvent>,
 }
 
 impl ChannelEventCollector {
@@ -100,10 +126,12 @@ impl ChannelEventCollector {
     pub fn new(
         collision_event_sender: Sender<CollisionEvent>,
         contact_force_event_sender: Sender<ContactForceEvent>,
+        fracture_event_sender: Sender<FractureEvent>,
     ) -> Self {
         Self {
             collision_event_sender,
             contact_force_event_sender,
+            fracture_event_sender,
         }
     }
 }
@@ -130,4 +158,13 @@ impl EventHandler for ChannelEventCollector {
         let result = ContactForceEvent::from_contact_pair(dt, contact_pair, total_force_magnitude);
         let _ = self.contact_force_event_sender.send(result);
     }
+
+    fn handle_fracture_event(
+        &self,
+        _bodies: &RigidBodySet,
+        _colliders: &ColliderSet,
+        event: FractureEvent,
+    ) {
+        let _ = self.fracture_event_sender.send(event);
+    }
 }
diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs
index bac67d2..ffa1bc8 100644
--- a/src/pipeline/mod.rs
+++ b/src/pipeline/mod.rs
@@ -1,11 +1,14 @@
 //! Structure for combining the various physics components to perform an actual simulation.
 
 pub use collision_pipeline::CollisionPipeline;
-pub use event_handler::{ActiveEvents, ChannelEventCollector, EventHandler};
+pub use event_handler::{ActiveEvents, ChannelEventCollector, EventHandler, FractureEvent};
 pub use physics_hooks::{ActiveHooks, ContactModificationContext, PairFilterContext, PhysicsHooks};
 pub use physics_pipeline::PhysicsPipeline;
 pub use query_pipeline::{QueryFilter, QueryFilterFlags, QueryPipeline, QueryPipelineMode};
 
+#[cfg(feature = "experimental-voxel-fracture")]
+pub use voxel_fracture_pipeline::{VoxelFractureMaterial, VoxelFracturePipeline};
+
 #[cfg(feature = "debug-render")]
 pub use self::debug_render_pipeline::{
     DebugColor, DebugRenderBackend, DebugRenderMode, DebugRenderObject, DebugRenderPipeline,
@@ -21,3 +24,5 @@ mod user_changes;
 
 #[cfg(feature = "debug-render")]
 mod debug_render_pipeline;
+#[cfg(feature = "experimental-voxel-fracture")]
+mod voxel_fracture_pipeline;
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 358efed..c25445f 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -191,11 +191,13 @@ impl PhysicsPipeline {
         }
 
         let mut manifolds = Vec::new();
+        let mut active_pairs = Vec::new();
         narrow_phase.select_active_contacts(
             islands,
             bodies,
             &mut self.contact_pair_indices,
             &mut manifolds,
+            &mut active_pairs,
             &mut self.manifold_indices,
         );
         impulse_joints.select_active_interactions(
@@ -507,6 +509,9 @@ impl PhysicsPipeline {
             };
 
         while remaining_substeps > 0 {
+            self.clear_modified_colliders(colliders, &mut modified_colliders);
+            removed_colliders.clear();
+
             // If there are more than one CCD substep, we need to split
             // the timestep into multiple intervals. First, estimate the
             // size of the time slice we will integrate for this substep.
@@ -581,7 +586,7 @@ impl PhysicsPipeline {
 
             // If CCD is enabled, execute the CCD motion clamping.
             if ccd_is_enabled {
-                // NOTE: don't the forces into account when updating the CCD active flags because
+                // NOTE: don't take the forces into account when updating the CCD active flags because
                 //       they have already been integrated into the velocities by the solver.
                 let ccd_active = ccd_solver.update_ccd_active_flags(
                     islands,
diff --git a/src/pipeline/voxel_fracture_pipeline.rs b/src/pipeline/voxel_fracture_pipeline.rs
new file mode 100644
index 0000000..e0a6852
--- /dev/null
+++ b/src/pipeline/voxel_fracture_pipeline.rs
@@ -0,0 +1,135 @@
+use crate::dynamics::{IntegrationParameters, IslandManager, RigidBodyBuilder, RigidBodySet};
+use crate::geometry::{Aabb, ColliderBuilder, ColliderHandle, ColliderSet, NarrowPhase};
+use crate::math::{Point, Real, Vector};
+use crate::pipeline::{EventHandler, FractureEvent};
+use parry::{shape::SharedShape, utils::hashmap::HashMap};
+
+#[derive(Copy, Clone, PartialEq, Debug)]
+pub struct VoxelFractureMaterial {
+    pub max_momentum: Real,
+    pub min_force: Real,
+    pub normal_dissipation: Real,
+    pub max_fracture_radius: Real,
+}
+
+impl Default for VoxelFractureMaterial {
+    fn default() -> Self {
+        Self {
+            max_momentum: 10.0,
+            min_force: 1.0,
+            normal_dissipation: 1.0,
+            max_fracture_radius: 1.0,
+        }
+    }
+}
+
+#[derive(Clone, Debug, Default)]
+pub struct VoxelFracturePipeline {
+    pub default_material: VoxelFractureMaterial,
+    // TODO: allow one material per voxel collider.
+}
+
+impl VoxelFracturePipeline {
+    pub fn step(
+        &self,
+        integration_parameters: &IntegrationParameters,
+        islands: &mut IslandManager,
+        narrow_phase: &NarrowPhase,
+        bodies: &mut RigidBodySet,
+        colliders: &mut ColliderSet,
+        events: &dyn EventHandler,
+    ) {
+        let mut fragments: HashMap<ColliderHandle, Vec<ColliderHandle>> = Default::default();
+
+        let mut num_pairs = 0;
+        let mut num_fract = 0;
+
+        // TODO: only iterate on the active pairs.
+        for active_pair in narrow_phase.contact_pairs() {
+            num_pairs += 1;
+            let co1 = &colliders[active_pair.collider1];
+            let co2 = &colliders[active_pair.collider2];
+
+            if co1.shape.as_voxels().is_none() && co2.shape.as_voxels().is_none() {
+                continue;
+            }
+
+            for manifold in &active_pair.manifolds {
+                if let Some(point) = &manifold.find_deepest_contact() {
+                    // Compute the fracture pattern at this contact.
+                    // momentum = force * dist => dist = momentum / force
+                    let force = point.data.impulse * integration_parameters.inv_dt();
+
+                    if force < self.default_material.min_force {
+                        continue;
+                    }
+
+                    let inv_force = crate::utils::inv(force);
+                    let fracture_radius = self.default_material.max_momentum * inv_force;
+
+                    if fracture_radius < self.default_material.max_fracture_radius {
+                        num_fract += 1;
+                        // Fracture the voxel model.
+                        let mut fracture_collider =
+                            |handle: ColliderHandle,
+                             local_n: Vector<Real>,
+                             local_p: Point<Real>| {
+                                let mut collider = &mut colliders[handle];
+
+                                if let Some(vox) = collider.shape.as_voxels() {
+                                    let fracture_normal = local_n;
+                                    let fracture_depth = fracture_radius; // TODO: find a good force-dependant depth.
+                                    let fracture_dir_id = fracture_normal.iamax();
+                                    let tangent_a = Vector::ith((fracture_dir_id + 1) % 3, 1.0);
+                                    let tangent_b = Vector::ith((fracture_dir_id + 2) % 3, 1.0);
+                                    let mut fracture_box = Aabb {
+                                        mins: local_p - (tangent_a + tangent_b) * fracture_radius,
+                                        maxs: local_p + (tangent_a + tangent_b) * fracture_radius,
+                                    };
+
+                                    if fracture_normal[fracture_dir_id] > 0.0 {
+                                        fracture_box.mins[fracture_dir_id] -= fracture_depth;
+                                    } else {
+                                        fracture_box.maxs[fracture_dir_id] += fracture_depth;
+                                    }
+
+                                    if let (Some(in_box), Some(rest)) =
+                                        vox.split_with_box(&fracture_box)
+                                    {
+                                        *collider.shape_mut().as_voxels_mut().unwrap() = rest;
+                                        // Create a new, dynamic body with the detached part.
+                                        let body = RigidBodyBuilder::dynamic()
+                                            .position(*collider.position());
+                                        let collider =
+                                            ColliderBuilder::new(SharedShape::new(in_box));
+                                        let body_handle = bodies.insert(body);
+                                        let fragment_handle = colliders.insert_with_parent(
+                                            collider,
+                                            body_handle,
+                                            bodies,
+                                        );
+
+                                        fragments
+                                            .entry(handle)
+                                            .or_insert_with(|| vec![])
+                                            .push(fragment_handle);
+                                    }
+                                }
+                            };
+
+                        fracture_collider(active_pair.collider1, manifold.local_n1, point.local_p1);
+                        fracture_collider(active_pair.collider2, manifold.local_n2, point.local_p2);
+                    }
+                }
+            }
+        }
+
+        for (handle, fragments) in fragments.drain() {
+            let event = FractureEvent {
+                fractured_collider: handle,
+                fragments,
+            };
+            events.handle_fracture_event(bodies, colliders, event);
+        }
+    }
+}