WQuadtree query: reduce the amount of allocations.

src/geometry/contact_generator/trimesh_shape_contact_generator.rs

@@ -108,7 +108,10 @@ fn do_generate_contacts(
         //     workspace.old_manifolds.len()
         // );
 
-        workspace.interferences = trimesh1.waabbs().intersect_aabb(&local_aabb2);
+        workspace.interferences.clear();
+        trimesh1
+            .waabbs()
+            .intersect_aabb(&local_aabb2, &mut workspace.interferences);
         workspace.local_aabb2 = local_aabb2;
     }
 

src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs

@@ -70,7 +70,10 @@ fn do_detect_proximity(
             &mut workspace.interferences,
         );
 
-        workspace.interferences = trimesh1.waabbs().intersect_aabb(&local_aabb2);
+        workspace.interferences.clear();
+        trimesh1
+            .waabbs()
+            .intersect_aabb(&local_aabb2, &mut workspace.interferences);
         workspace.local_aabb2 = local_aabb2;
     }
 

src/pipeline/query_pipeline.rs

@@ -64,15 +64,10 @@ impl QueryPipeline {
         ray: &Ray,
         max_toi: f32,
     ) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> {
-        // let t0 = instant::now();
+        // TODO: avoid allocation?
-        let inter = self.quadtree.cast_ray(ray, max_toi);
+        let mut inter = Vec::new();
-        // println!(
+        self.quadtree.cast_ray(ray, max_toi, &mut inter);
-        //     "Found {} interferences in time {}.",
-        //     inter.len(),
-        //     instant::now() - t0
-        // );
 
-        // let t0 = instant::now();
         let mut best = f32::MAX;
         let mut result = None;
 
@@ -85,7 +80,6 @@ impl QueryPipeline {
                 }
             }
         }
-        // println!("Cast time: {}", instant::now() - t0);
 
         result
     }
@@ -107,8 +101,12 @@ impl QueryPipeline {
         max_toi: f32,
         mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool,
     ) {
-        // FIXME: this is a brute-force approach.
+        // TODO: avoid allocation?
-        for (handle, collider) in colliders.iter() {
+        let mut inter = Vec::new();
+        self.quadtree.cast_ray(ray, max_toi, &mut inter);
+
+        for handle in inter {
+            let collider = &colliders[handle];
             if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) {
                 if !callback(handle, collider, inter) {
                     return;