diff --git a/Cargo.lock b/Cargo.lock
index 564fe931..635518ce 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -147,6 +147,12 @@ dependencies = [
  "num-traits",
 ]
 
+[[package]]
+name = "arbitrary-chunks"
+version = "0.4.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2ad8689a486416c401ea15715a4694de30054248ec627edbf31f49cb64ee4086"
+
 [[package]]
 name = "arrayref"
 version = "0.3.9"
@@ -305,8 +311,10 @@ dependencies = [
  "itertools 0.13.0",
  "libm",
  "nalgebra",
+ "obvhs",
  "parry2d",
  "parry2d-f64",
+ "rand 0.9.1",
  "serde",
  "slab",
  "smallvec",
@@ -334,6 +342,7 @@ dependencies = [
  "itertools 0.13.0",
  "libm",
  "nalgebra",
+ "obvhs",
  "parry3d",
  "parry3d-f64",
  "serde",
@@ -923,7 +932,7 @@ dependencies = [
  "glam 0.30.8",
  "itertools 0.14.0",
  "libm",
- "rand",
+ "rand 0.8.5",
  "rand_distr",
  "serde",
  "smallvec",
@@ -1547,6 +1556,16 @@ version = "0.1.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "0d8c1fef690941d3e7788d328517591fecc684c084084702d6ff1641e993699a"
 
+[[package]]
+name = "block-pseudorand"
+version = "0.1.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2097358495d244a0643746f4d13eedba4608137008cf9dec54e53a3b700115a6"
+dependencies = [
+ "chiapos-chacha8",
+ "nanorand",
+]
+
 [[package]]
 name = "block2"
 version = "0.5.1"
@@ -1663,6 +1682,15 @@ version = "0.2.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "613afe47fcd5fac7ccf1db93babcb082c5994d996f20b8b159f2ad1658eb5724"
 
+[[package]]
+name = "chiapos-chacha8"
+version = "0.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "33f8be573a85f6c2bc1b8e43834c07e32f95e489b914bf856c0549c3c269cd0a"
+dependencies = [
+ "rayon",
+]
+
 [[package]]
 name = "ciborium"
 version = "0.2.2"
@@ -2458,8 +2486,13 @@ dependencies = [
  "approx",
  "bytemuck",
  "libm",
+<<<<<<< HEAD
  "rand",
  "serde_core",
+=======
+ "rand 0.8.5",
+ "serde",
+>>>>>>> 5baf1b3 (Make spatial queries generic over colliders (WIP))
 ]
 
 [[package]]
@@ -3034,6 +3067,12 @@ dependencies = [
  "syn",
 ]
 
+[[package]]
+name = "nanorand"
+version = "0.6.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "729eb334247daa1803e0a094d0a5c55711b85571179f5ec6e53eccfdf7008958"
+
 [[package]]
 name = "ndk"
 version = "0.9.0"
@@ -3386,6 +3425,19 @@ dependencies = [
  "objc2-foundation",
 ]
 
+[[package]]
+name = "obvhs"
+version = "0.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "96831b0c2386d37ee14731a382554956523a1fc202a26aff9fa0b34a6450d583"
+dependencies = [
+ "bytemuck",
+ "glam",
+ "half",
+ "rayon",
+ "rdst",
+]
+
 [[package]]
 name = "offset-allocator"
 version = "0.2.0"
@@ -3588,6 +3640,12 @@ dependencies = [
  "thiserror 2.0.17",
 ]
 
+[[package]]
+name = "partition"
+version = "0.1.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "947f833aaa585cf12b8ec7c0476c98784c49f33b861376ffc84ed92adebf2aba"
+
 [[package]]
 name = "paste"
 version = "1.0.15"
@@ -3832,8 +3890,24 @@ version = "0.9.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "6db2770f06117d490610c7488547d543617b21bfa07796d7a12f6f1bd53850d1"
 dependencies = [
+<<<<<<< HEAD
  "rand_chacha",
  "rand_core",
+=======
+ "libc",
+ "rand_chacha 0.3.1",
+ "rand_core 0.6.4",
+]
+
+[[package]]
+name = "rand"
+version = "0.9.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9fbfd9d094a40bf3ae768db9361049ace4c0e04a4fd6b359518bd7b73a73dd97"
+dependencies = [
+ "rand_chacha 0.9.0",
+ "rand_core 0.9.3",
+>>>>>>> 5baf1b3 (Make spatial queries generic over colliders (WIP))
 ]
 
 [[package]]
@@ -3843,7 +3917,17 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "d3022b5f1df60f26e1ffddd6c66e8aa15de382ae63b3a0c1bfc0e4d3e3f325cb"
 dependencies = [
  "ppv-lite86",
- "rand_core",
+ "rand_core 0.6.4",
+]
+
+[[package]]
+name = "rand_chacha"
+version = "0.9.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d3022b5f1df60f26e1ffddd6c66e8aa15de382ae63b3a0c1bfc0e4d3e3f325cb"
+dependencies = [
+ "ppv-lite86",
+ "rand_core 0.9.3",
 ]
 
 [[package]]
@@ -3855,6 +3939,15 @@ dependencies = [
  "getrandom",
 ]
 
+[[package]]
+name = "rand_core"
+version = "0.9.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "99d9a13982dcf210057a8a78572b2217b667c3beacbf3a0d8b454f6f82837d38"
+dependencies = [
+ "getrandom 0.3.3",
+]
+
 [[package]]
 name = "rand_distr"
 version = "0.5.1"
@@ -3862,7 +3955,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "6a8615d50dcf34fa31f7ab52692afec947c4dd0ab803cc87cb3b0b4570ff7463"
 dependencies = [
  "num-traits",
- "rand",
+ "rand 0.8.5",
 ]
 
 [[package]]
@@ -3909,6 +4002,21 @@ dependencies = [
  "crossbeam-utils",
 ]
 
+[[package]]
+name = "rdst"
+version = "0.20.14"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "6e7970b4e577b76a96d5e56b5f6662b66d1a4e1f5bb026ee118fc31b373c2752"
+dependencies = [
+ "arbitrary-chunks",
+ "block-pseudorand",
+ "criterion",
+ "partition",
+ "rayon",
+ "tikv-jemallocator",
+ "voracious_radix_sort",
+]
+
 [[package]]
 name = "read-fonts"
 version = "0.35.0"
@@ -4406,6 +4514,26 @@ dependencies = [
  "cfg-if",
 ]
 
+[[package]]
+name = "tikv-jemalloc-sys"
+version = "0.5.4+5.3.0-patched"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9402443cb8fd499b6f327e40565234ff34dbda27460c5b47db0db77443dd85d1"
+dependencies = [
+ "cc",
+ "libc",
+]
+
+[[package]]
+name = "tikv-jemallocator"
+version = "0.5.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "965fe0c26be5c56c94e38ba547249074803efd52adfb66de62107d95aab3eaca"
+dependencies = [
+ "libc",
+ "tikv-jemalloc-sys",
+]
+
 [[package]]
 name = "tinytemplate"
 version = "1.2.1"
@@ -4676,6 +4804,15 @@ version = "0.9.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a"
 
+[[package]]
+name = "voracious_radix_sort"
+version = "1.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "446e7ffcb6c27a71d05af7e51ef2ee5b71c48424b122a832f2439651e1914899"
+dependencies = [
+ "rayon",
+]
+
 [[package]]
 name = "walkdir"
 version = "2.5.0"
diff --git a/crates/avian2d/Cargo.toml b/crates/avian2d/Cargo.toml
index c23e8846..dbe8cc1d 100644
--- a/crates/avian2d/Cargo.toml
+++ b/crates/avian2d/Cargo.toml
@@ -33,6 +33,7 @@ parallel = [
     "bevy/multi_threaded",
     "parry2d?/parallel",
     "parry2d-f64?/parallel",
+    "obvhs/parallel",
 ]
 enhanced-determinism = [
     "dep:libm",
@@ -92,6 +93,7 @@ bevy_heavy = { version = "0.3" }
 bevy_transform_interpolation = { version = "0.3" }
 libm = { version = "0.2", optional = true }
 approx = "0.5"
+obvhs = "0.2"
 parry2d = { version = "0.25", optional = true }
 parry2d-f64 = { version = "0.25", optional = true }
 nalgebra = { version = "0.34", features = ["convert-glam030"], optional = true }
@@ -117,6 +119,7 @@ glam = { version = "0.30", features = ["bytemuck"] }
 bytemuck = "1.19"
 criterion = { version = "0.5", features = ["html_reports"] }
 bevy_mod_debugdump = { version = "0.14" }
+rand = "0.9"
 
 [lints]
 workspace = true
diff --git a/crates/avian2d/examples/custom_collider.rs b/crates/avian2d/examples/custom_collider.rs
index 89708bad..7f35c3fa 100644
--- a/crates/avian2d/examples/custom_collider.rs
+++ b/crates/avian2d/examples/custom_collider.rs
@@ -61,7 +61,7 @@ impl AnyCollider for CircleCollider {
         &self,
         position: Vector,
         _: impl Into<Rotation>,
-        _: AabbContext<Self::Context>,
+        _: SingleContext<Self::Context>,
     ) -> ColliderAabb {
         ColliderAabb::new(position, Vector::splat(self.radius))
     }
@@ -77,7 +77,7 @@ impl AnyCollider for CircleCollider {
         _rotation2: impl Into<Rotation>,
         prediction_distance: Scalar,
         manifolds: &mut Vec<ContactManifold>,
-        _: ContactManifoldContext<Self::Context>,
+        _: PairContext<Self::Context>,
     ) {
         // Clear the previous manifolds.
         manifolds.clear();
diff --git a/crates/avian2d/examples/spatial_query.rs b/crates/avian2d/examples/spatial_query.rs
new file mode 100644
index 00000000..ce9255ac
--- /dev/null
+++ b/crates/avian2d/examples/spatial_query.rs
@@ -0,0 +1,306 @@
+#![expect(clippy::unnecessary_cast)]
+
+use avian2d::{
+    collision::{
+        collider::{PairContext, QueryCollider, QueryShapeCastHit, SingleContext},
+        contact_types::PackedFeatureId,
+    },
+    math::*,
+    prelude::*,
+};
+use bevy::{color::palettes::tailwind::GRAY_400, prelude::*, render::camera::ScalingMode};
+use examples_common_2d::ExampleCommonPlugin;
+use ops::FloatPow;
+use rand::Rng;
+
+fn main() {
+    App::new()
+        .add_plugins((
+            DefaultPlugins,
+            ExampleCommonPlugin,
+            PhysicsPlugins::default()
+                .with_length_unit(10.0)
+                .build()
+                .disable::<ColliderBackendPlugin<Collider>>()
+                .disable::<NarrowPhasePlugin<Collider>>()
+                .disable::<SpatialQueryPlugin<Collider>>(),
+            PhysicsDebugPlugin::default(),
+            ColliderBackendPlugin::<CircleCollider>::default(),
+            NarrowPhasePlugin::<CircleCollider>::default(),
+            SpatialQueryPlugin::<CircleCollider>::default(),
+        ))
+        .insert_gizmo_config(
+            PhysicsGizmos::default().with_aabb_color(GRAY_400.into()),
+            GizmoConfig {
+                line: GizmoLineConfig {
+                    width: 0.5,
+                    ..default()
+                },
+                ..default()
+            },
+        )
+        .add_systems(Startup, setup)
+        .add_systems(FixedUpdate, move_random)
+        .add_systems(Update, cast_ray)
+        .run();
+}
+
+/// A simplified custom collider.
+#[derive(Component, Clone, Copy, Debug, Reflect)]
+struct CircleCollider {
+    radius: Scalar,
+    unscaled_radius: Scalar,
+    scale: Scalar,
+}
+
+impl CircleCollider {
+    fn new(radius: Scalar) -> Self {
+        Self {
+            radius,
+            unscaled_radius: radius,
+            scale: 1.0,
+        }
+    }
+}
+
+impl AnyCollider for CircleCollider {
+    type Context = ();
+
+    fn aabb_with_context(
+        &self,
+        position: Vector,
+        _: impl Into<Rotation>,
+        _: SingleContext<()>,
+    ) -> ColliderAabb {
+        ColliderAabb::new(position, Vector::splat(self.radius))
+    }
+
+    fn contact_manifolds_with_context(
+        &self,
+        other: &Self,
+        position1: Vector,
+        rotation1: impl Into<Rotation>,
+        position2: Vector,
+        _: impl Into<Rotation>,
+        prediction_distance: Scalar,
+        manifolds: &mut Vec<ContactManifold>,
+        _: PairContext<()>,
+    ) {
+        // Clear the previous manifolds.
+        manifolds.clear();
+
+        let rotation1: Rotation = rotation1.into();
+
+        let inv_rotation1 = rotation1.inverse();
+        let delta_pos = inv_rotation1 * (position2 - position1);
+
+        let distance_squared = delta_pos.length_squared();
+        let sum_radius = self.radius + other.radius;
+
+        if distance_squared < (sum_radius + prediction_distance).powi(2) {
+            let local_normal1 = if distance_squared != 0.0 {
+                delta_pos.normalize_or_zero()
+            } else {
+                Vector::X
+            };
+            let local_point1 = local_normal1 * self.radius;
+            let normal = rotation1 * local_normal1;
+
+            let separation = distance_squared.sqrt() - sum_radius;
+
+            let point1 = rotation1 * local_point1;
+            let anchor1 = point1 + normal * separation * 0.5;
+            let anchor2 = anchor1 + (position1 - position2);
+            let world_point = position1 + anchor1;
+
+            let point = ContactPoint::new(anchor1, anchor2, world_point, -separation)
+                .with_feature_ids(PackedFeatureId::face(0), PackedFeatureId::face(0));
+
+            manifolds.push(ContactManifold::new([point], rotation1 * local_normal1));
+        }
+    }
+}
+
+impl QueryCollider for CircleCollider {
+    type CastShape = Circle;
+    type Shape = Circle;
+
+    fn ray_hit(&self, ray: obvhs::ray::Ray, _: bool, _: SingleContext<()>) -> f32 {
+        let offset = ray.origin;
+        let projected = offset.dot(ray.direction);
+        let closest_point = offset - projected * ray.direction;
+        let distance_squared = self.radius.squared() - closest_point.length_squared();
+        if distance_squared < 0.
+            || ops::copysign(projected.squared(), -projected) < -distance_squared
+        {
+            f32::INFINITY
+        } else {
+            let toi = -projected - ops::sqrt(distance_squared);
+            if toi > ray.tmax {
+                f32::INFINITY
+            } else {
+                toi.max(0.)
+            }
+        }
+    }
+
+    fn ray_normal(&self, point: Vector, _: Dir2, _: bool, _: SingleContext<()>) -> Vec2 {
+        point.normalize_or(Vec2::Y)
+    }
+
+    fn shape_cast(
+        &self,
+        shape: &Self::CastShape,
+        _: Rotation,
+        local_origin: Vec2,
+        local_dir: Dir2,
+        (tmin, tmax): (f32, f32),
+        context: SingleContext<()>,
+    ) -> Option<QueryShapeCastHit> {
+        let mut c = self.clone();
+        c.radius += shape.radius;
+        let ray = obvhs::ray::Ray::new(
+            local_origin.extend(0.).into(),
+            local_dir.extend(0.).into(),
+            tmin,
+            tmax,
+        );
+        let hit = c.ray_hit(ray, true, context);
+        if hit < f32::INFINITY {
+            Some(QueryShapeCastHit {
+                distance: hit,
+                // TODO
+                point: default(),
+                normal: default(),
+            })
+        } else {
+            None
+        }
+    }
+
+    fn shape_intersection(
+        &self,
+        shape: &Self::CastShape,
+        shape_rotation: Rotation,
+        local_origin: Vec2,
+        _: SingleContext<()>,
+    ) -> bool {
+        todo!()
+    }
+
+    fn closest_point(&self, point: Vec2, solid: bool, _: SingleContext<()>) -> Vec2 {
+        todo!()
+    }
+
+    fn contains_point(&self, point: Vec2, _: SingleContext<()>) -> bool {
+        todo!()
+    }
+}
+
+// Implement mass computation for the collider shape.
+// This is needed for physics to behave correctly.
+impl ComputeMassProperties2d for CircleCollider {
+    fn mass(&self, density: f32) -> f32 {
+        // In 2D, the Z length is assumed to be `1.0`, so volume == area.
+        let volume = core::f32::consts::PI * self.radius.powi(2) as f32;
+        density * volume
+    }
+
+    fn unit_angular_inertia(&self) -> f32 {
+        // Angular inertia for a circle, assuming a mass of `1.0`.
+        self.radius.powi(2) as f32 / 2.0
+    }
+
+    fn center_of_mass(&self) -> Vec2 {
+        Vec2::ZERO
+    }
+}
+
+// Note: This circle collider only supports uniform scaling.
+impl ScalableCollider for CircleCollider {
+    fn scale(&self) -> Vector {
+        Vector::splat(self.scale)
+    }
+
+    fn set_scale(&mut self, scale: Vector, _detail: u32) {
+        // For non-unifprm scaling, this would need to be converted to an ellipse collider or a convex hull.
+        self.scale = scale.max_element();
+        self.radius = self.unscaled_radius * scale.max_element();
+    }
+}
+
+const X_COUNT: i32 = 50;
+const Y_COUNT: i32 = 30;
+const PARTICLE_RADIUS: f32 = 5.;
+const SPACING_FACTOR: f32 = 3.;
+
+fn setup(mut commands: Commands) {
+    commands.spawn((
+        Camera2d,
+        Projection::Orthographic(OrthographicProjection {
+            scaling_mode: ScalingMode::FixedVertical {
+                viewport_height: SPACING_FACTOR * PARTICLE_RADIUS * (Y_COUNT + 1) as f32,
+            },
+            ..OrthographicProjection::default_2d()
+        }),
+    ));
+
+    for x in -X_COUNT / 2..=X_COUNT / 2 {
+        for y in -Y_COUNT / 2..=Y_COUNT / 2 {
+            commands.spawn((
+                RigidBody::Kinematic,
+                Transform::from_xyz(
+                    x as f32 * SPACING_FACTOR * PARTICLE_RADIUS,
+                    y as f32 * SPACING_FACTOR * PARTICLE_RADIUS,
+                    0.0,
+                ),
+                CircleCollider::new(PARTICLE_RADIUS.adjust_precision()),
+                CollisionLayers::new(LayerMask::DEFAULT, LayerMask::NONE),
+            ));
+        }
+    }
+}
+
+fn move_random(window: Single<&Window>, mut query: Query<(&Position, &mut LinearVelocity)>) {
+    let mut rng = rand::rng();
+    for (pos, mut lin_vel) in query.iter_mut() {
+        let max_y = (Y_COUNT + 1) as f32 * 0.5 * SPACING_FACTOR * PARTICLE_RADIUS;
+        let aspect_ratio = window.resolution.width() / window.resolution.height();
+        let out_of_x = pos.x.abs() > max_y * aspect_ratio - PARTICLE_RADIUS;
+        let out_of_y = pos.y.abs() > max_y - PARTICLE_RADIUS;
+        if out_of_x {
+            lin_vel.x = -lin_vel.x.copysign(pos.x);
+        }
+        if out_of_y {
+            lin_vel.y = -lin_vel.y.copysign(pos.y);
+        }
+        if rng.random::<f32>() < 0.15 {
+            lin_vel.0 += Vec2::new(rng.random_range(-0.25..0.25), rng.random_range(-0.25..0.25));
+        }
+    }
+}
+
+fn cast_ray(mut gizmos: Gizmos, spatial_query: SpatialQuery<CircleCollider>, time: Res<Time>) {
+    let t = time.elapsed_secs() * 0.1;
+    let direction = Dir2::new_unchecked(Vec2::new(t.sin(), t.cos()));
+    gizmos.circle_2d(Vec2::ZERO, 3., Color::WHITE);
+    gizmos.line_2d(Vec2::ZERO, direction * 10000., Color::WHITE);
+    spatial_query.shape_hits_callback(
+        &Circle { radius: 7. },
+        Vec2::ZERO,
+        0.,
+        direction,
+        &ShapeCastConfig::from_max_distance(10000.),
+        &SpatialQueryFilter::default(),
+        &mut |hit| {
+            let (pos, _, col) = spatial_query.colliders.get(hit.entity).unwrap();
+            gizmos.circle_2d(
+                Isometry2d::from_translation(**pos),
+                col.radius,
+                Color::srgb(1., 0.4, 0.4),
+            );
+            gizmos.circle_2d(direction * hit.distance, 7., Color::srgb(0.5, 1., 1.));
+            true
+        },
+    );
+}
diff --git a/crates/avian3d/Cargo.toml b/crates/avian3d/Cargo.toml
index 4015c09a..524aacc3 100644
--- a/crates/avian3d/Cargo.toml
+++ b/crates/avian3d/Cargo.toml
@@ -34,6 +34,7 @@ parallel = [
     "bevy/multi_threaded",
     "parry3d?/parallel",
     "parry3d-f64?/parallel",
+    "obvhs/parallel",
 ]
 enhanced-determinism = [
     "dep:libm",
@@ -94,6 +95,7 @@ bevy_heavy = { version = "0.3" }
 bevy_transform_interpolation = { version = "0.3" }
 libm = { version = "0.2", optional = true }
 approx = "0.5"
+obvhs = "0.2"
 parry3d = { version = "0.25", optional = true }
 parry3d-f64 = { version = "0.25", optional = true }
 nalgebra = { version = "0.34", features = ["convert-glam030"], optional = true }
diff --git a/src/collision/collider/backend.rs b/src/collision/collider/backend.rs
index d6dea8bc..25f322ec 100644
--- a/src/collision/collider/backend.rs
+++ b/src/collision/collider/backend.rs
@@ -19,6 +19,8 @@ use bevy::{
 };
 use mass_properties::{MassPropertySystems, components::RecomputeMassProperties};
 
+use super::SingleContext;
+
 /// A plugin for handling generic collider backend logic.
 ///
 /// - Initializes colliders, handles [`ColliderConstructor`] and [`ColliderConstructorHierarchy`].
@@ -197,7 +199,7 @@ impl<C: ScalableCollider> Plugin for ColliderBackendPlugin<C> {
              length_unit: Res<PhysicsLengthUnit>,
              collider_context: StaticSystemParam<C::Context>| {
                 let contact_tolerance = length_unit.0 * narrow_phase_config.contact_tolerance;
-                let aabb_context = AabbContext::new(trigger.entity, &*collider_context);
+                let aabb_context = SingleContext::new(trigger.entity, &*collider_context);
 
                 if let Ok((collider, pos, rot, collision_margin, mut aabb)) =
                     query.get_mut(trigger.entity)
@@ -558,7 +560,7 @@ fn update_aabb<C: AnyCollider>(
             speculative_margin.map_or(default_speculative_margin, |margin| margin.0)
         };
 
-        let context = AabbContext::new(entity, &*collider_context);
+        let context = SingleContext::new(entity, &*collider_context);
 
         if speculative_margin <= 0.0 {
             *aabb = collider
diff --git a/src/collision/collider/mod.rs b/src/collision/collider/mod.rs
index 4796f7e7..b3f8fe71 100644
--- a/src/collision/collider/mod.rs
+++ b/src/collision/collider/mod.rs
@@ -1,6 +1,6 @@
 //! Components, traits, and plugins related to collider functionality.
 
-use crate::prelude::*;
+use crate::{physics_transform::RotationValue, prelude::*};
 use bevy::{
     ecs::{
         component::Mutable,
@@ -8,6 +8,7 @@ use bevy::{
         system::{ReadOnlySystemParam, SystemParam, SystemParamItem},
     },
     prelude::*,
+    reflect::GetTypeRegistration,
 };
 use derive_more::From;
 
@@ -55,14 +56,14 @@ pub trait IntoCollider<C: AnyCollider> {
 
 /// Context necessary to calculate [`ColliderAabb`]s for an [`AnyCollider`]
 #[derive(Deref)]
-pub struct AabbContext<'a, 'w, 's, T: ReadOnlySystemParam> {
+pub struct SingleContext<'a, 'w, 's, T: ReadOnlySystemParam> {
     /// The entity for which the aabb is being calculated
     pub entity: Entity,
     #[deref]
     item: &'a SystemParamItem<'w, 's, T>,
 }
 
-impl<T: ReadOnlySystemParam> Clone for AabbContext<'_, '_, '_, T> {
+impl<T: ReadOnlySystemParam> Clone for SingleContext<'_, '_, '_, T> {
     fn clone(&self) -> Self {
         Self {
             entity: self.entity,
@@ -71,14 +72,14 @@ impl<T: ReadOnlySystemParam> Clone for AabbContext<'_, '_, '_, T> {
     }
 }
 
-impl<'a, 'w, 's, T: ReadOnlySystemParam> AabbContext<'a, 'w, 's, T> {
+impl<'a, 'w, 's, T: ReadOnlySystemParam> SingleContext<'a, 'w, 's, T> {
     /// Construct an [`AabbContext`]
     pub fn new(entity: Entity, item: &'a <T as SystemParam>::Item<'w, 's>) -> Self {
         Self { entity, item }
     }
 }
 
-impl AabbContext<'_, '_, '_, ()> {
+impl SingleContext<'_, '_, '_, ()> {
     fn fake() -> Self {
         Self {
             entity: Entity::PLACEHOLDER,
@@ -89,7 +90,7 @@ impl AabbContext<'_, '_, '_, ()> {
 
 /// Context necessary to calculate [`ContactManifold`]s for a set of [`AnyCollider`]
 #[derive(Deref)]
-pub struct ContactManifoldContext<'a, 'w, 's, T: ReadOnlySystemParam> {
+pub struct PairContext<'a, 'w, 's, T: ReadOnlySystemParam> {
     /// The first collider entity involved in the contact.
     pub entity1: Entity,
     /// The second collider entity involved in the contact.
@@ -98,7 +99,7 @@ pub struct ContactManifoldContext<'a, 'w, 's, T: ReadOnlySystemParam> {
     item: &'a SystemParamItem<'w, 's, T>,
 }
 
-impl<'a, 'w, 's, T: ReadOnlySystemParam> ContactManifoldContext<'a, 'w, 's, T> {
+impl<'a, 'w, 's, T: ReadOnlySystemParam> PairContext<'a, 'w, 's, T> {
     /// Construct a [`ContactManifoldContext`]
     pub fn new(
         entity1: Entity,
@@ -113,7 +114,7 @@ impl<'a, 'w, 's, T: ReadOnlySystemParam> ContactManifoldContext<'a, 'w, 's, T> {
     }
 }
 
-impl ContactManifoldContext<'_, '_, '_, ()> {
+impl PairContext<'_, '_, '_, ()> {
     fn fake() -> Self {
         Self {
             entity1: Entity::PLACEHOLDER,
@@ -215,7 +216,7 @@ pub trait AnyCollider: Component<Mutability = Mutable> + ComputeMassProperties {
         &self,
         position: Vector,
         rotation: impl Into<Rotation>,
-        context: AabbContext<Self::Context>,
+        context: SingleContext<Self::Context>,
     ) -> ColliderAabb;
 
     /// Computes the swept [Axis-Aligned Bounding Box](ColliderAabb) of the collider.
@@ -233,7 +234,7 @@ pub trait AnyCollider: Component<Mutability = Mutable> + ComputeMassProperties {
         start_rotation: impl Into<Rotation>,
         end_position: Vector,
         end_rotation: impl Into<Rotation>,
-        context: AabbContext<Self::Context>,
+        context: SingleContext<Self::Context>,
     ) -> ColliderAabb {
         self.aabb_with_context(start_position, start_rotation, context.clone())
             .merged(self.aabb_with_context(end_position, end_rotation, context))
@@ -254,7 +255,7 @@ pub trait AnyCollider: Component<Mutability = Mutable> + ComputeMassProperties {
         rotation2: impl Into<Rotation>,
         prediction_distance: Scalar,
         manifolds: &mut Vec<ContactManifold>,
-        context: ContactManifoldContext<Self::Context>,
+        context: PairContext<Self::Context>,
     );
 }
 
@@ -266,7 +267,7 @@ pub trait SimpleCollider: AnyCollider<Context = ()> {
     ///
     /// See [`AnyCollider::aabb_with_context`] for collider types with non-empty [`AnyCollider::Context`]
     fn aabb(&self, position: Vector, rotation: impl Into<Rotation>) -> ColliderAabb {
-        self.aabb_with_context(position, rotation, AabbContext::fake())
+        self.aabb_with_context(position, rotation, SingleContext::fake())
     }
 
     /// Computes the swept [Axis-Aligned Bounding Box](ColliderAabb) of the collider.
@@ -286,7 +287,7 @@ pub trait SimpleCollider: AnyCollider<Context = ()> {
             start_rotation,
             end_position,
             end_rotation,
-            AabbContext::fake(),
+            SingleContext::fake(),
         )
     }
 
@@ -314,7 +315,7 @@ pub trait SimpleCollider: AnyCollider<Context = ()> {
             rotation2,
             prediction_distance,
             manifolds,
-            ContactManifoldContext::fake(),
+            PairContext::fake(),
         )
     }
 }
@@ -341,6 +342,122 @@ pub trait ScalableCollider: AnyCollider {
     }
 }
 
+// TODO: Figure out if we can use BoundedShape for AnyCollider
+
+/// A trait to provide the AABB of the shape
+pub trait BoundedShape<Context: for<'w, 's> ReadOnlySystemParam<Item<'w, 's>: Send + Sync>> {
+    /// Get the AABB for the shape
+    fn shape_aabb(
+        &self,
+        position: Vector,
+        rotation: RotationValue,
+        context: &SystemParamItem<Context>,
+    ) -> ColliderAabb;
+}
+
+#[cfg(feature = "2d")]
+impl<
+    T: bevy_math::bounding::Bounded2d,
+    C: for<'w, 's> ReadOnlySystemParam<Item<'w, 's>: Send + Sync>,
+> BoundedShape<C> for T
+{
+    fn shape_aabb(&self, position: Vec2, rotation: f32, _: &SystemParamItem<C>) -> ColliderAabb {
+        let aabb = self.aabb_2d(Isometry2d::new(position, Rot2::radians(rotation)));
+        ColliderAabb {
+            min: aabb.min.into(),
+            max: aabb.max.into(),
+        }
+    }
+}
+
+#[cfg(feature = "3d")]
+impl<
+    T: bevy_math::bounding::Bounded3d,
+    C: for<'w, 's> ReadOnlySystemParam<Item<'w, 's>: Send + Sync>,
+> BoundedShape<C> for T
+{
+    fn shape_aabb(&self, position: Vec3, rotation: Quat, _: &SystemParamItem<C>) -> ColliderAabb {
+        let aabb = self.aabb_3d(Isometry3d::new(position, rotation));
+        ColliderAabb {
+            min: aabb.min.into(),
+            max: aabb.max.into(),
+        }
+    }
+}
+
+/// Like [`ShapeCastHit`], but without the [`Entity`], should only be used by the [`QueryCollider`] trait
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct QueryShapeCastHit {
+    /// How far the shape travelled before the initial hit.
+    pub distance: Scalar,
+
+    /// The hit point in world space.
+    ///
+    /// If the shapes are penetrating, this will be the midpoint between the closest points
+    /// on the surfaces of the two shapes.
+    pub point: Vector,
+
+    /// The outward surface normal on the hit shape at the hit point in world space.
+    pub normal: Vector,
+}
+
+/// A trait to implement [`SpatialQuery`] support for an [`AnyCollider`]
+pub trait QueryCollider: AnyCollider + core::fmt::Debug + TypePath + GetTypeRegistration {
+    // TODO: Experiment with removing these Self::Contexts here
+
+    /// The shape used for shape casting against this collider type
+    type CastShape: BoundedShape<Self::Context> + Default + Sync + Send;
+    /// The shape used for shape intersections with this collider type
+    type Shape: BoundedShape<Self::Context> + Sync + Send;
+
+    /// Get the distance at which the ray hits the collider, [`f32::INFINITY`] if no hit.
+    fn ray_hit(
+        &self,
+        ray: obvhs::ray::Ray,
+        solid: bool,
+        context: SingleContext<Self::Context>,
+    ) -> f32;
+    // TODO: Should we combine ray_normal and ray_hit?
+    /// Get the normal for the hit at the provided point.
+    fn ray_normal(
+        &self,
+        point: Vector,
+        direction: Dir,
+        solid: bool,
+        context: SingleContext<Self::Context>,
+    ) -> Vector;
+
+    /// Get the shape cast hit against the collider, if any
+    fn shape_cast(
+        &self,
+        shape: &Self::CastShape,
+        shape_rotation: Rotation,
+        local_origin: Vector,
+        local_dir: Dir,
+        range: (f32, f32),
+        context: SingleContext<Self::Context>,
+    ) -> Option<QueryShapeCastHit>;
+
+    /// Check if the provided shape intersects with this collider
+    fn shape_intersection(
+        &self,
+        shape: &Self::Shape,
+        shape_rotation: Rotation,
+        local_origin: Vector,
+        context: SingleContext<Self::Context>,
+    ) -> bool;
+
+    /// Get the closest point on the collider to the provided point
+    fn closest_point(
+        &self,
+        point: Vector,
+        solid: bool,
+        context: SingleContext<Self::Context>,
+    ) -> Vector;
+    /// Check if the collider touches the provided point
+    fn contains_point(&self, point: Vector, context: SingleContext<Self::Context>) -> bool;
+}
+
 /// A marker component that indicates that a [collider](Collider) is disabled
 /// and should not detect collisions or be included in spatial queries.
 ///
@@ -509,7 +626,7 @@ impl ColliderAabb {
             min: self.min - amount,
             max: self.max + amount,
         };
-        debug_assert!(b.min.cmple(b.max).all());
+        debug_assert!(b == Self::INVALID || b.min.cmple(b.max).all());
         b
     }
 
@@ -520,7 +637,7 @@ impl ColliderAabb {
             min: self.min + amount,
             max: self.max - amount,
         };
-        debug_assert!(b.min.cmple(b.max).all());
+        debug_assert!(b == Self::INVALID || b.min.cmple(b.max).all());
         b
     }
 
diff --git a/src/collision/collider/parry/mod.rs b/src/collision/collider/parry/mod.rs
index 1ee020e1..2da4f044 100644
--- a/src/collision/collider/parry/mod.rs
+++ b/src/collision/collider/parry/mod.rs
@@ -21,6 +21,8 @@ use parry::{
     shape::{RoundShape, SharedShape, TypedShape, Voxels},
 };
 
+use super::{BoundedShape, PairContext, QueryCollider, QueryShapeCastHit, SingleContext};
+
 impl<T: IntoCollider<Collider>> From<T> for Collider {
     fn from(value: T) -> Self {
         value.collider()
@@ -355,7 +357,8 @@ pub type TrimeshBuilderError = parry::shape::TriMeshBuilderError;
 /// or [`Collider::shape_scaled()`] methods.
 ///
 /// `Collider` is currently not `Reflect`. If you need to reflect it, you can use [`ColliderConstructor`] as a workaround.
-#[derive(Clone, Component, Debug)]
+#[derive(Clone, Component, Debug, Reflect)]
+#[reflect(from_reflect = false)]
 #[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
 #[require(
     ColliderMarker,
@@ -366,11 +369,13 @@ pub type TrimeshBuilderError = parry::shape::TriMeshBuilderError;
 )]
 pub struct Collider {
     /// The raw unscaled collider shape.
+    #[reflect(ignore)]
     shape: SharedShape,
     /// The scaled version of the collider shape.
     ///
     /// If the scale is `Vector::ONE`, this will be `None` and `unscaled_shape`
     /// will be used instead.
+    #[reflect(ignore)]
     scaled_shape: SharedShape,
     /// The global scale used for the collider shape.
     scale: Vector,
@@ -406,7 +411,7 @@ impl AnyCollider for Collider {
         &self,
         position: Vector,
         rotation: impl Into<Rotation>,
-        _: AabbContext<Self::Context>,
+        _: SingleContext<Self::Context>,
     ) -> ColliderAabb {
         let aabb = self
             .shape_scaled()
@@ -426,7 +431,7 @@ impl AnyCollider for Collider {
         rotation2: impl Into<Rotation>,
         prediction_distance: Scalar,
         manifolds: &mut Vec<ContactManifold>,
-        _: ContactManifoldContext<Self::Context>,
+        _: PairContext<Self::Context>,
     ) {
         contact_query::contact_manifolds(
             self,
@@ -441,6 +446,68 @@ impl AnyCollider for Collider {
     }
 }
 
+impl BoundedShape<()> for Collider {
+    fn shape_aabb(&self, position: Vector, rotation: RotationValue, _: &()) -> ColliderAabb {
+        self.aabb(position, rotation)
+    }
+}
+
+impl QueryCollider for Collider {
+    // TODO: Both of these should probably be SharedShape instead of Collider which has a scaled vs unscaled shape
+    type CastShape = Self;
+    type Shape = Self;
+
+    fn ray_hit(&self, ray: obvhs::ray::Ray, solid: bool, _: SingleContext<()>) -> f32 {
+        _ = (ray, solid);
+        todo!()
+    }
+
+    fn ray_normal(
+        &self,
+        point: Vector,
+        direction: Dir,
+        solid: bool,
+        _: SingleContext<()>,
+    ) -> Vector {
+        _ = (point, direction, solid);
+        todo!()
+    }
+
+    fn shape_cast(
+        &self,
+        shape: &Self,
+        shape_rotation: Rotation,
+        local_origin: Vector,
+        local_dir: Dir,
+        range: (f32, f32),
+        _: SingleContext<()>,
+    ) -> Option<QueryShapeCastHit> {
+        _ = (shape, shape_rotation, local_origin, local_dir, range);
+        todo!()
+    }
+
+    fn shape_intersection(
+        &self,
+        shape: &Self,
+        shape_rotation: Rotation,
+        local_origin: Vector,
+        _: SingleContext<()>,
+    ) -> bool {
+        _ = (shape, shape_rotation, local_origin);
+        todo!()
+    }
+
+    fn closest_point(&self, point: Vector, solid: bool, _: SingleContext<()>) -> Vector {
+        _ = (point, solid);
+        todo!()
+    }
+
+    fn contains_point(&self, point: Vector, _: SingleContext<()>) -> bool {
+        _ = point;
+        todo!()
+    }
+}
+
 // TODO: `bevy_heavy` supports computing the individual mass properties efficiently for Bevy's primitive shapes,
 //       but Parry doesn't support it for its own shapes, so we have to compute all mass properties in each method :(
 #[cfg(feature = "2d")]
diff --git a/src/collision/mod.rs b/src/collision/mod.rs
index a7d22bcf..5f1c8927 100644
--- a/src/collision/mod.rs
+++ b/src/collision/mod.rs
@@ -82,10 +82,9 @@ pub mod prelude {
     #[cfg(all(feature = "collider-from-mesh", feature = "default-collider"))]
     pub use super::collider::ColliderCachePlugin;
     pub use super::collider::{
-        AabbContext, AnyCollider, ColliderAabb, ColliderBackendPlugin, ColliderDisabled,
-        ColliderMarker, CollidingEntities, CollisionLayers, CollisionMargin,
-        ContactManifoldContext, IntoCollider, LayerMask, PhysicsLayer, ScalableCollider, Sensor,
-        SimpleCollider,
+        AnyCollider, ColliderAabb, ColliderBackendPlugin, ColliderDisabled, ColliderMarker,
+        CollidingEntities, CollisionLayers, CollisionMargin, IntoCollider, LayerMask, PairContext,
+        PhysicsLayer, ScalableCollider, Sensor, SimpleCollider, SingleContext,
         collider_hierarchy::{ColliderHierarchyPlugin, ColliderOf, RigidBodyColliders},
         collider_transform::{ColliderTransform, ColliderTransformPlugin},
     };
diff --git a/src/collision/narrow_phase/system_param.rs b/src/collision/narrow_phase/system_param.rs
index 6809c757..b95a938e 100644
--- a/src/collision/narrow_phase/system_param.rs
+++ b/src/collision/narrow_phase/system_param.rs
@@ -695,11 +695,8 @@ impl<C: AnyCollider> NarrowPhase<'_, '_, C> {
                 // TODO: It'd be good to persist the manifolds and let Parry match contacts.
                 //       This isn't currently done because it requires using Parry's contact manifold type.
                 // Compute the contact manifolds using the effective speculative margin.
-                let context = ContactManifoldContext::new(
-                    collider1.entity,
-                    collider2.entity,
-                    collider_context,
-                );
+                let context =
+                    PairContext::new(collider1.entity, collider2.entity, collider_context);
                 collider1.shape.contact_manifolds_with_context(
                     collider2.shape,
                     collider1.position.0,
@@ -711,7 +708,6 @@ impl<C: AnyCollider> NarrowPhase<'_, '_, C> {
                     context,
                 );
 
-                // Transform and prune contact data.
                 contacts.manifolds.iter_mut().for_each(|manifold| {
                     // Set the initial surface properties.
                     manifold.friction = friction;
diff --git a/src/debug_render/gizmos.rs b/src/debug_render/gizmos.rs
index dc47d655..0cced99c 100644
--- a/src/debug_render/gizmos.rs
+++ b/src/debug_render/gizmos.rs
@@ -56,7 +56,7 @@ pub trait PhysicsGizmoExt {
         origin: Vector,
         direction: Dir,
         max_distance: Scalar,
-        hits: &[RayHitData],
+        hits: &[RayCastHit],
         ray_color: Color,
         point_color: Color,
         normal_color: Color,
@@ -76,7 +76,7 @@ pub trait PhysicsGizmoExt {
         shape_rotation: impl Into<Rotation>,
         direction: Dir,
         max_distance: Scalar,
-        hits: &[ShapeHitData],
+        hits: &[ShapeCastHit],
         ray_color: Color,
         shape_color: Color,
         point_color: Color,
@@ -479,7 +479,7 @@ impl PhysicsGizmoExt for Gizmos<'_, '_, PhysicsGizmos> {
         origin: Vector,
         direction: Dir,
         max_distance: Scalar,
-        hits: &[RayHitData],
+        hits: &[RayCastHit],
         ray_color: Color,
         point_color: Color,
         normal_color: Color,
@@ -531,7 +531,7 @@ impl PhysicsGizmoExt for Gizmos<'_, '_, PhysicsGizmos> {
         shape_rotation: impl Into<Rotation>,
         direction: Dir,
         max_distance: Scalar,
-        hits: &[ShapeHitData],
+        hits: &[ShapeCastHit],
         ray_color: Color,
         shape_color: Color,
         point_color: Color,
@@ -563,14 +563,14 @@ impl PhysicsGizmoExt for Gizmos<'_, '_, PhysicsGizmos> {
         for hit in hits {
             // Draw hit point
             #[cfg(feature = "2d")]
-            self.circle_2d(hit.point1.f32(), 0.1 * length_unit as f32, point_color);
+            self.circle_2d(hit.point.f32(), 0.1 * length_unit as f32, point_color);
             #[cfg(feature = "3d")]
-            self.sphere(hit.point1.f32(), 0.1 * length_unit as f32, point_color);
+            self.sphere(hit.point.f32(), 0.1 * length_unit as f32, point_color);
 
             // Draw hit normal as arrow
             self.draw_arrow(
-                hit.point1,
-                hit.point1 + hit.normal1 * 0.5 * length_unit,
+                hit.point,
+                hit.point + hit.normal * 0.5 * length_unit,
                 0.1 * length_unit,
                 normal_color,
             );
diff --git a/src/debug_render/mod.rs b/src/debug_render/mod.rs
index 2fb2b97e..4f4b4900 100644
--- a/src/debug_render/mod.rs
+++ b/src/debug_render/mod.rs
@@ -421,7 +421,7 @@ fn debug_render_raycasts(
     any(feature = "parry-f32", feature = "parry-f64")
 ))]
 fn debug_render_shapecasts(
-    query: Query<(&ShapeCaster, &ShapeHits)>,
+    query: Query<(&ShapeCaster<Collider>, &ShapeHits)>,
     mut gizmos: Gizmos<PhysicsGizmos>,
     store: Res<GizmoConfigStore>,
     length_unit: Res<PhysicsLengthUnit>,
diff --git a/src/lib.rs b/src/lib.rs
index 42e5a739..3a32abbe 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -815,7 +815,7 @@ impl PluginGroup for PhysicsPlugins {
         ))]
         let builder = builder
             .add(ColliderBackendPlugin::<Collider>::new(self.schedule))
-            .add(NarrowPhasePlugin::<Collider>::default());
+            .add(SpatialQueryPlugin::<Collider>::default());
 
         // Add solver plugins.
         let builder = builder.add_group(SolverPlugins::new_with_length_unit(self.length_unit));
@@ -823,7 +823,6 @@ impl PluginGroup for PhysicsPlugins {
         builder
             .add(BroadPhasePlugin::<()>::default())
             .add(JointPlugin)
-            .add(SpatialQueryPlugin)
             .add(PhysicsTransformPlugin::new(self.schedule))
             .add(PhysicsInterpolationPlugin::default())
     }
diff --git a/src/physics_transform/transform.rs b/src/physics_transform/transform.rs
index 2f6037f6..60a6bd59 100644
--- a/src/physics_transform/transform.rs
+++ b/src/physics_transform/transform.rs
@@ -632,6 +632,19 @@ impl core::ops::Mul<Vector3> for Rotation {
     }
 }
 
+#[cfg(feature = "2d")]
+impl core::ops::Mul<Vec3A> for Rotation {
+    type Output = Vec3A;
+
+    fn mul(self, rhs: Vec3A) -> Self::Output {
+        Vec3A::new(
+            rhs.x * self.cos - rhs.y * self.sin,
+            rhs.x * self.sin + rhs.y * self.cos,
+            rhs.z,
+        )
+    }
+}
+
 #[cfg(feature = "2d")]
 impl core::ops::Mul<&Vector3> for Rotation {
     type Output = Vector3;
@@ -826,9 +839,18 @@ impl core::ops::Mul<Vector> for Rotation {
     }
 }
 
+#[cfg(feature = "3d")]
+impl core::ops::Mul<Vec3A> for Rotation {
+    type Output = Vec3A;
+
+    fn mul(self, vector: Vec3A) -> Self::Output {
+        self.0 * vector
+    }
+}
+
 #[cfg(feature = "3d")]
 impl core::ops::Mul for Rotation {
-    type Output = Rotation;
+    type Output = Self;
 
     fn mul(self, rhs: Self) -> Self::Output {
         Self(self.0 * rhs.0)
diff --git a/src/picking/mod.rs b/src/picking/mod.rs
index 8b02d6a7..41195b06 100644
--- a/src/picking/mod.rs
+++ b/src/picking/mod.rs
@@ -16,6 +16,7 @@ Note that in 3D, only the closest intersection will be reported."
 )]
 
 use crate::{
+    collision::collider::QueryCollider,
     diagnostics::{PhysicsDiagnostics, impl_diagnostic_paths},
     prelude::*,
 };
@@ -29,6 +30,7 @@ use bevy::{
 };
 
 use core::time::Duration;
+use std::marker::PhantomData;
 
 use bevy::{
     diagnostic::DiagnosticPath,
@@ -57,13 +59,23 @@ impl_diagnostic_paths! {
 }
 
 /// Adds the [physics picking](crate::picking) backend to your app, enabling picking for [colliders](Collider).
-#[derive(Clone, Default)]
-pub struct PhysicsPickingPlugin;
+#[derive(Clone)]
+pub struct PhysicsPickingPlugin<C: QueryCollider> {
+    phantom: PhantomData<C>,
+}
+
+impl<C: QueryCollider> Default for PhysicsPickingPlugin<C> {
+    fn default() -> Self {
+        Self {
+            phantom: PhantomData,
+        }
+    }
+}
 
-impl Plugin for PhysicsPickingPlugin {
+impl<C: QueryCollider> Plugin for PhysicsPickingPlugin<C> {
     fn build(&self, app: &mut App) {
         app.init_resource::<PhysicsPickingSettings>()
-            .add_systems(PreUpdate, update_hits.in_set(PickingSystems::Backend));
+            .add_systems(PreUpdate, update_hits::<C>.in_set(PickingSystems::Backend));
     }
 
     fn finish(&self, app: &mut App) {
@@ -137,7 +149,7 @@ const DEFAULT_FILTER_REF: &PhysicsPickingFilter =
     &PhysicsPickingFilter(SpatialQueryFilter::DEFAULT);
 
 /// Queries for collider intersections with pointers using [`PhysicsPickingSettings`] and sends [`PointerHits`] events.
-pub fn update_hits(
+pub fn update_hits<C: QueryCollider>(
     picking_cameras: Query<(
         &Camera,
         Option<&PhysicsPickingFilter>,
@@ -147,7 +159,7 @@ pub fn update_hits(
     pickables: Query<&Pickable>,
     marked_targets: Query<&PhysicsPickable>,
     backend_settings: Res<PhysicsPickingSettings>,
-    spatial_query: SpatialQuery,
+    spatial_query: SpatialQuery<C>,
     mut output_events: MessageWriter<PointerHits>,
     mut diagnostics: ResMut<PhysicsPickingDiagnostics>,
 ) {
@@ -171,7 +183,7 @@ pub fn update_hits(
             spatial_query.point_intersections_callback(
                 ray.origin.truncate().adjust_precision(),
                 &filter.0,
-                |entity| {
+                &mut |entity| {
                     let marker_requirement =
                         !backend_settings.require_markers || marked_targets.get(entity).is_ok();
 
diff --git a/src/spatial_query/bvh_ext.rs b/src/spatial_query/bvh_ext.rs
new file mode 100644
index 00000000..8fd02c21
--- /dev/null
+++ b/src/spatial_query/bvh_ext.rs
@@ -0,0 +1,266 @@
+use bevy_math::Vec3A;
+use obvhs::{
+    aabb::Aabb,
+    bvh2::{Bvh2, RayTraversal},
+    heapstack::HeapStack,
+    ray::{Ray, RayHit},
+};
+
+pub(crate) trait BvhExt {
+    fn cast_traverse<F: FnMut(&Ray, usize) -> f32>(
+        &self,
+        ray: Ray,
+        hit: &mut RayHit,
+        shape: Aabb,
+        intersection_fn: F,
+    ) -> bool;
+
+    fn cast_traverse_dynamic<F: FnMut(&Ray, usize) -> f32>(
+        &self,
+        state: &mut RayTraversal,
+        hit: &mut RayHit,
+        shape: Aabb,
+        intersection_fn: F,
+    ) -> bool;
+
+    fn dist_traverse<F: FnMut(&Vec3A, usize) -> f32>(
+        &self,
+        pos: Vec3A,
+        hit: &mut RayHit,
+        dist_fn: F,
+    ) -> bool;
+
+    fn dist_traverse_dynamic<F: FnMut(&Vec3A, usize) -> f32>(
+        &self,
+        state: &mut DistTraversal,
+        hit: &mut RayHit,
+        dist_fn: F,
+    ) -> bool;
+
+    fn point_traverse_dynamic<F: FnMut(&Vec3A, usize) -> bool>(
+        &self,
+        state: &mut PointTraversal,
+        hit: &mut RayHit,
+        contains_fn: F,
+    ) -> bool;
+}
+
+impl BvhExt for Bvh2 {
+    fn cast_traverse<F: FnMut(&Ray, usize) -> f32>(
+        &self,
+        ray: Ray,
+        hit: &mut RayHit,
+        shape: Aabb,
+        mut intersection_fn: F,
+    ) -> bool {
+        let mut state = self.new_ray_traversal(ray);
+        while self.cast_traverse_dynamic(&mut state, hit, shape, &mut intersection_fn) {}
+        hit.t < ray.tmax // Note this is valid since traverse_with_stack does not mutate the ray
+    }
+
+    fn cast_traverse_dynamic<F: FnMut(&Ray, usize) -> f32>(
+        &self,
+        state: &mut RayTraversal,
+        hit: &mut RayHit,
+        shape: Aabb,
+        mut intersection_fn: F,
+    ) -> bool {
+        loop {
+            while state.primitive_count > 0 {
+                let primitive_id = state.current_primitive_index;
+                state.current_primitive_index += 1;
+                state.primitive_count -= 1;
+                let t = intersection_fn(&state.ray, primitive_id as usize);
+                if t < state.ray.tmax {
+                    hit.primitive_id = primitive_id;
+                    hit.t = t;
+                    state.ray.tmax = t;
+                    // Yield when we hit a primitive
+                    return true;
+                }
+            }
+            if let Some(current_node_index) = state.stack.pop() {
+                let node = &self.nodes[*current_node_index as usize];
+                let modified_aabb = Aabb {
+                    min: node.aabb.min - shape.max,
+                    max: node.aabb.max - shape.min,
+                };
+                if modified_aabb.intersect_ray(&state.ray) >= state.ray.tmax {
+                    continue;
+                }
+
+                if node.is_leaf() {
+                    state.primitive_count = node.prim_count;
+                    state.current_primitive_index = node.first_index;
+                } else {
+                    state.stack.push(node.first_index);
+                    state.stack.push(node.first_index + 1);
+                }
+            } else {
+                // Returns false when there are no more primitives to test.
+                // This doesn't mean we never hit one along the way though. (and yielded then)
+                return false;
+            }
+        }
+    }
+
+    fn dist_traverse<F: FnMut(&Vec3A, usize) -> f32>(
+        &self,
+        pos: Vec3A,
+        hit: &mut RayHit,
+        mut dist_fn: F,
+    ) -> bool {
+        let mut stack = HeapStack::new_with_capacity(self.max_depth.unwrap_or(96));
+        if !self.nodes.is_empty() {
+            stack.push(0);
+        }
+        let mut state = DistTraversal {
+            stack,
+            point: pos,
+            min_dist_sq: f32::INFINITY,
+            current_primitive_index: 0,
+            primitive_count: 0,
+        };
+        while self.dist_traverse_dynamic(&mut state, hit, &mut dist_fn) {}
+        hit.t < f32::INFINITY
+    }
+
+    fn dist_traverse_dynamic<F: FnMut(&Vec3A, usize) -> f32>(
+        &self,
+        state: &mut DistTraversal,
+        hit: &mut RayHit,
+        mut dist_fn: F,
+    ) -> bool {
+        loop {
+            while state.primitive_count > 0 {
+                let primitive_id = state.current_primitive_index;
+                state.current_primitive_index += 1;
+                state.primitive_count -= 1;
+                let d = dist_fn(&state.point, primitive_id as usize);
+                let d_sq = d * d;
+                if d_sq < state.min_dist_sq {
+                    hit.primitive_id = primitive_id;
+                    hit.t = d;
+                    state.min_dist_sq = d_sq;
+                    // Yield when we hit a primitive
+                    return true;
+                }
+            }
+            if let Some(current_node_index) = state.stack.pop() {
+                let node = &self.nodes[*current_node_index as usize];
+                if node.aabb.distance_squared(state.point) >= state.min_dist_sq {
+                    continue;
+                }
+
+                if node.is_leaf() {
+                    state.primitive_count = node.prim_count;
+                    state.current_primitive_index = node.first_index;
+                } else {
+                    state.stack.push(node.first_index);
+                    state.stack.push(node.first_index + 1);
+                }
+            } else {
+                // Returns false when there are no more primitives to test.
+                // This doesn't mean we never hit one along the way though. (and yielded then)
+                return false;
+            }
+        }
+    }
+
+    fn point_traverse_dynamic<F: FnMut(&Vec3A, usize) -> bool>(
+        &self,
+        state: &mut PointTraversal,
+        hit: &mut RayHit,
+        mut contains_fn: F,
+    ) -> bool {
+        loop {
+            while state.primitive_count > 0 {
+                let primitive_id = state.current_primitive_index;
+                state.current_primitive_index += 1;
+                state.primitive_count -= 1;
+                if contains_fn(&state.point, primitive_id as usize) {
+                    hit.primitive_id = primitive_id;
+                    // Yield when we hit a primitive
+                    return true;
+                }
+            }
+            if let Some(current_node_index) = state.stack.pop() {
+                let node = &self.nodes[*current_node_index as usize];
+                if !node.aabb.contains_point(state.point) {
+                    continue;
+                }
+
+                if node.is_leaf() {
+                    state.primitive_count = node.prim_count;
+                    state.current_primitive_index = node.first_index;
+                } else {
+                    state.stack.push(node.first_index);
+                    state.stack.push(node.first_index + 1);
+                }
+            } else {
+                // Returns false when there are no more primitives to test.
+                // This doesn't mean we never hit one along the way though. (and yielded then)
+                return false;
+            }
+        }
+    }
+}
+
+pub struct DistTraversal {
+    pub stack: HeapStack<u32>,
+    pub point: Vec3A,
+    pub min_dist_sq: f32,
+    pub current_primitive_index: u32,
+    pub primitive_count: u32,
+}
+
+trait DistAabb {
+    fn distance_squared(&self, p: Vec3A) -> f32;
+}
+
+impl DistAabb for Aabb {
+    fn distance_squared(&self, p: Vec3A) -> f32 {
+        let closest = p.clamp(self.min, self.max);
+        return closest.distance_squared(p);
+    }
+}
+
+pub struct PointTraversal {
+    pub stack: HeapStack<u32>,
+    pub point: Vec3A,
+    pub current_primitive_index: u32,
+    pub primitive_count: u32,
+}
+
+impl PointTraversal {
+    pub fn new(bvh: &Bvh2, pos: Vec3A) -> Self {
+        let mut stack = HeapStack::new_with_capacity(bvh.max_depth.unwrap_or(96));
+        if !bvh.nodes.is_empty() {
+            stack.push(0);
+        }
+        PointTraversal {
+            stack,
+            point: pos,
+            current_primitive_index: 0,
+            primitive_count: 0,
+        }
+    }
+}
+
+pub trait RayExt {
+    fn transformed(self, pos: super::Position, rot: super::Rotation) -> Self;
+}
+
+impl RayExt for Ray {
+    fn transformed(mut self, pos: super::Position, rot: super::Rotation) -> Self {
+        let inv_rot = rot.inverse();
+        self.direction = inv_rot * self.direction;
+        self.inv_direction = inv_rot * self.inv_direction;
+        #[cfg(feature = "2d")]
+        let pos = pos.extend(0.);
+        #[cfg(feature = "3d")]
+        let pos = *pos;
+        self.origin = inv_rot * (self.origin - Vec3A::from(pos));
+        self
+    }
+}
diff --git a/src/spatial_query/mod.rs b/src/spatial_query/mod.rs
index 9a48e210..eb611a75 100644
--- a/src/spatial_query/mod.rs
+++ b/src/spatial_query/mod.rs
@@ -150,42 +150,39 @@
 //!
 //! To specify which colliders should be considered in the query, use a [spatial query filter](`SpatialQueryFilter`).
 
-#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
+mod bvh_ext;
+
 mod pipeline;
 mod query_filter;
 mod ray_caster;
-#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
 mod shape_caster;
-#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
 mod system_param;
 
 mod diagnostics;
+use std::marker::PhantomData;
+
 pub use diagnostics::SpatialQueryDiagnostics;
 
-#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
 pub use pipeline::*;
 pub use query_filter::*;
 pub use ray_caster::*;
-#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
 pub use shape_caster::*;
-#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
 pub use system_param::*;
 
-use crate::prelude::*;
+use crate::{collision::collider::QueryCollider, prelude::*};
 use bevy::prelude::*;
 
-/// Initializes the [`SpatialQueryPipeline`] resource and handles component-based [spatial queries](spatial_query)
-/// like [raycasting](spatial_query#raycasting) and [shapecasting](spatial_query#shapecasting) with
-/// [`RayCaster`] and [`ShapeCaster`].
-pub struct SpatialQueryPlugin;
+pub use obvhs;
+
+#[derive(SystemSet, Clone, Copy, PartialEq, Eq, Hash, Debug)]
+struct UpdatePipeline;
 
-impl Plugin for SpatialQueryPlugin {
+struct UniversalSpatialQueryPlugin;
+
+impl Plugin for UniversalSpatialQueryPlugin {
     fn build(&self, app: &mut App) {
-        #[cfg(all(
-            feature = "default-collider",
-            any(feature = "parry-f32", feature = "parry-f64")
-        ))]
         app.init_resource::<SpatialQueryPipeline>();
+        app.allow_ambiguous_resource::<SpatialQueryDiagnostics>();
 
         let physics_schedule = app
             .get_schedule_mut(PhysicsSchedule)
@@ -193,23 +190,50 @@ impl Plugin for SpatialQueryPlugin {
 
         physics_schedule.add_systems(
             (
-                update_ray_caster_positions,
-                #[cfg(all(
-                    feature = "default-collider",
-                    any(feature = "parry-f32", feature = "parry-f64")
-                ))]
-                (
-                    update_shape_caster_positions,
-                    update_spatial_query_pipeline,
-                    raycast,
-                    shapecast,
-                )
-                    .chain(),
+                update_ray_caster_positions.before(UpdatePipeline),
+                update_spatial_query_pipeline.in_set(UpdatePipeline),
             )
                 .chain()
                 .in_set(PhysicsStepSystems::SpatialQuery),
         );
     }
+}
+
+/// Initializes the [`SpatialQueryPipeline`] resource and handles component-based [spatial queries](spatial_query)
+/// like [raycasting](spatial_query#raycasting) and [shapecasting](spatial_query#shapecasting) with
+/// [`RayCaster`] and [`ShapeCaster`].
+pub struct SpatialQueryPlugin<C: QueryCollider> {
+    phantom: PhantomData<C>,
+}
+
+impl<C: QueryCollider> Default for SpatialQueryPlugin<C> {
+    fn default() -> Self {
+        Self {
+            phantom: PhantomData,
+        }
+    }
+}
+
+impl<C: QueryCollider> Plugin for SpatialQueryPlugin<C> {
+    fn build(&self, app: &mut App) {
+        if !app.is_plugin_added::<UniversalSpatialQueryPlugin>() {
+            app.add_plugins(UniversalSpatialQueryPlugin);
+        }
+
+        let physics_schedule = app
+            .get_schedule_mut(PhysicsSchedule)
+            .expect("add PhysicsSchedule first");
+
+        physics_schedule.add_systems((
+            update_shape_caster_positions::<C>
+                .after(update_ray_caster_positions)
+                .before(UpdatePipeline)
+                .in_set(PhysicsStepSet::SpatialQuery),
+            (raycast::<C>, shapecast::<C>)
+                .after(UpdatePipeline)
+                .in_set(PhysicsStepSet::SpatialQuery),
+        ));
+    }
 
     fn finish(&self, app: &mut App) {
         // Register timer diagnostics for spatial queries.
@@ -218,19 +242,18 @@ impl Plugin for SpatialQueryPlugin {
 }
 
 /// Updates the [`SpatialQueryPipeline`].
-#[cfg(all(
-    feature = "default-collider",
-    any(feature = "parry-f32", feature = "parry-f64")
-))]
 pub fn update_spatial_query_pipeline(
-    mut spatial_query: SpatialQuery,
-    mut diagnostics: ResMut<SpatialQueryDiagnostics>,
+    mut spatial_query: ResMut<SpatialQueryPipeline>,
+    aabbs: Query<(Entity, Option<&CollisionLayers>, &ColliderAabb)>,
+    diagnostics: Option<ResMut<SpatialQueryDiagnostics>>,
 ) {
     let start = crate::utils::Instant::now();
 
-    spatial_query.update_pipeline();
+    spatial_query.update(aabbs.iter());
 
-    diagnostics.update_pipeline = start.elapsed();
+    if let Some(mut diagnostics) = diagnostics {
+        diagnostics.update_pipeline = start.elapsed();
+    }
 }
 
 type RayCasterPositionQueryComponents = (
@@ -300,19 +323,17 @@ fn update_ray_caster_positions(
     }
 }
 
-#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
-type ShapeCasterPositionQueryComponents = (
-    &'static mut ShapeCaster,
+type ShapeCasterPositionQueryComponents<C> = (
+    &'static mut ShapeCaster<C>,
     Option<&'static Position>,
     Option<&'static Rotation>,
     Option<&'static ChildOf>,
     Option<&'static GlobalTransform>,
 );
 
-#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
 #[allow(clippy::type_complexity)]
-fn update_shape_caster_positions(
-    mut shape_casters: Query<ShapeCasterPositionQueryComponents>,
+fn update_shape_caster_positions<C: QueryCollider>(
+    mut shape_casters: Query<ShapeCasterPositionQueryComponents<C>>,
     parents: Query<
         (
             Option<&Position>,
@@ -396,11 +417,10 @@ fn update_shape_caster_positions(
     }
 }
 
-#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
-fn raycast(
+fn raycast<C: QueryCollider>(
     mut rays: Query<(Entity, &mut RayCaster, &mut RayHits)>,
-    spatial_query: SpatialQuery,
-    mut diagnostics: ResMut<SpatialQueryDiagnostics>,
+    spatial_query: SpatialQuery<C>,
+    diagnostics: Option<ResMut<SpatialQueryDiagnostics>>,
 ) {
     let start = crate::utils::Instant::now();
 
@@ -412,14 +432,15 @@ fn raycast(
         }
     }
 
-    diagnostics.update_ray_casters = start.elapsed();
+    if let Some(mut diagnostics) = diagnostics {
+        diagnostics.update_ray_casters = start.elapsed();
+    }
 }
 
-#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
-fn shapecast(
-    mut shape_casters: Query<(Entity, &ShapeCaster, &mut ShapeHits)>,
-    spatial_query: SpatialQuery,
-    mut diagnostics: ResMut<SpatialQueryDiagnostics>,
+fn shapecast<C: QueryCollider>(
+    mut shape_casters: Query<(Entity, &ShapeCaster<C>, &mut ShapeHits)>,
+    spatial_query: SpatialQuery<C>,
+    diagnostics: Option<ResMut<SpatialQueryDiagnostics>>,
 ) {
     let start = crate::utils::Instant::now();
 
@@ -431,5 +452,7 @@ fn shapecast(
         }
     }
 
-    diagnostics.update_shape_casters = start.elapsed();
+    if let Some(mut diagnostics) = diagnostics {
+        diagnostics.update_shape_casters = start.elapsed();
+    }
 }
diff --git a/src/spatial_query/pipeline.rs b/src/spatial_query/pipeline.rs
index ccf38483..e53809a4 100644
--- a/src/spatial_query/pipeline.rs
+++ b/src/spatial_query/pipeline.rs
@@ -1,45 +1,59 @@
-use alloc::sync::Arc;
-
-use crate::prelude::*;
+use crate::{collision::collider::QueryShapeCastHit, prelude::*};
 use bevy::prelude::*;
-use parry::{
-    bounding_volume::{Aabb, BoundingVolume},
-    math::Isometry,
-    partitioning::{Bvh, BvhBuildStrategy, BvhNode},
-    query::{
-        DefaultQueryDispatcher, QueryDispatcher, RayCast, ShapeCastOptions,
-        details::NormalConstraints,
-    },
-    shape::{CompositeShape, CompositeShapeRef, Shape, TypedCompositeShape},
+use obvhs::{
+    BvhBuildParams,
+    aabb::Aabb,
+    bvh2::{Bvh2, builder::build_bvh2},
+    ray::RayHit,
 };
 
-// TODO: It'd be nice not to store so much duplicate data.
-//       Should we just query the ECS?
+use spatial_query::bvh_ext::{BvhExt, PointTraversal};
+
+/// A proxy for an entity used for the [`SpatialQueryPipeline`]
 #[derive(Clone)]
-pub(crate) struct BvhProxyData {
+pub struct BvhProxy {
+    /// The entity the proxy is for
     pub entity: Entity,
-    pub isometry: Isometry<Scalar>,
-    pub collider: Collider,
+    /// The collision layers of the entity
     pub layers: CollisionLayers,
+    /// The AABB of the entity
+    pub aabb: ColliderAabb,
+}
+
+#[cfg(feature = "3d")]
+impl obvhs::Boundable for BvhProxy {
+    fn aabb(&self) -> obvhs::aabb::Aabb {
+        obvhs::aabb::Aabb {
+            min: self.aabb.min.into(),
+            max: self.aabb.max.into(),
+        }
+    }
+}
+
+#[cfg(feature = "2d")]
+impl obvhs::Boundable for BvhProxy {
+    fn aabb(&self) -> obvhs::aabb::Aabb {
+        obvhs::aabb::Aabb {
+            min: self.aabb.min.extend(-1.).into(),
+            max: self.aabb.max.extend(1.).into(),
+        }
+    }
 }
 
 /// A resource for the spatial query pipeline.
 ///
-/// The pipeline maintains a quaternary bounding volume hierarchy `Bvh` of the world's colliders
+/// The pipeline maintains a quaternary bounding volume hierarchy `Qbvh` of the world's colliders
 /// as an acceleration structure for spatial queries.
 #[derive(Resource, Clone)]
 pub struct SpatialQueryPipeline {
-    pub(crate) bvh: Bvh,
-    pub(crate) dispatcher: Arc<dyn QueryDispatcher>,
-    // TODO: Store the proxies as `Bvh` leaf data.
-    pub(crate) proxies: Vec<BvhProxyData>,
+    pub(crate) bvh: Bvh2,
+    pub(crate) proxies: Vec<BvhProxy>,
 }
 
 impl Default for SpatialQueryPipeline {
     fn default() -> Self {
         Self {
-            bvh: Bvh::new(),
-            dispatcher: Arc::new(DefaultQueryDispatcher),
+            bvh: Bvh2::default(),
             proxies: Vec::default(),
         }
     }
@@ -51,95 +65,41 @@ impl SpatialQueryPipeline {
         SpatialQueryPipeline::default()
     }
 
-    pub(crate) fn as_composite_shape_internal<'a>(
-        &'a self,
-        query_filter: &'a SpatialQueryFilter,
-    ) -> QueryPipelineAsCompositeShape<'a> {
-        QueryPipelineAsCompositeShape {
-            pipeline: self,
-            query_filter,
-        }
+    /// Get the BVH of the pipeline
+    pub fn bvh(&self) -> &Bvh2 {
+        &self.bvh
     }
 
-    /// Creates a parry [`TypedCompositeShape`] for this pipeline.
-    /// Can be used to implement custom spatial queries
-    pub fn as_composite_shape<'a>(
-        &'a self,
-        query_filter: &'a SpatialQueryFilter,
-    ) -> impl TypedCompositeShape {
-        self.as_composite_shape_internal(query_filter)
-    }
-
-    pub(crate) fn as_composite_shape_with_predicate_internal<'a: 'b, 'b>(
-        &'a self,
-        query_filter: &'a SpatialQueryFilter,
-        predicate: &'a dyn Fn(Entity) -> bool,
-    ) -> QueryPipelineAsCompositeShapeWithPredicate<'a, 'b> {
-        QueryPipelineAsCompositeShapeWithPredicate {
-            pipeline: self,
-            query_filter,
-            predicate,
-        }
-    }
-
-    /// Creates a parry [`TypedCompositeShape`] for this pipeline, with a predicate.
-    /// Can be used to implement custom spatial queries
-    pub fn as_composite_shape_with_predicate<'a>(
-        &'a self,
-        query_filter: &'a SpatialQueryFilter,
-        predicate: &'a dyn Fn(Entity) -> bool,
-    ) -> impl TypedCompositeShape {
-        self.as_composite_shape_with_predicate_internal(query_filter, predicate)
+    /// Get a specific proxy using an index returned from the BVH
+    pub fn get_proxy(&self, bvh_idx: usize) -> &BvhProxy {
+        &self.proxies[self.bvh.primitive_indices[bvh_idx] as usize]
     }
 
     /// Updates the associated acceleration structures with a new set of entities.
     pub fn update<'a>(
         &mut self,
-        colliders: impl Iterator<
-            Item = (
-                Entity,
-                &'a Position,
-                &'a Rotation,
-                &'a Collider,
-                &'a CollisionLayers,
-            ),
-        >,
+        colliders: impl Iterator<Item = (Entity, Option<&'a CollisionLayers>, &'a ColliderAabb)>,
     ) {
-        self.update_internal(
-            colliders.map(
-                |(entity, position, rotation, collider, layers)| BvhProxyData {
-                    entity,
-                    isometry: make_isometry(position.0, *rotation),
-                    collider: collider.clone(),
-                    layers: *layers,
-                },
-            ),
-        )
-    }
-
-    // TODO: Incremental updates.
-    fn update_internal(&mut self, proxies: impl Iterator<Item = BvhProxyData>) {
-        self.proxies.clear();
-        self.proxies.extend(proxies);
-
-        let aabbs = self.proxies.iter().enumerate().map(|(i, proxy)| {
-            (
-                i,
-                proxy.collider.shape_scaled().compute_aabb(&proxy.isometry),
-            )
-        });
+        let colliders = colliders
+            .filter(|&(_, _, &aabb)| aabb != ColliderAabb::INVALID)
+            .map(|(entity, layers, &aabb)| BvhProxy {
+                entity,
+                layers: layers.map_or(CollisionLayers::default(), |layers| *layers),
+                aabb,
+            })
+            .collect();
 
-        self.bvh = Bvh::from_iter(BvhBuildStrategy::Binned, aabbs);
+        self.update_internal(colliders);
     }
 
-    /// Get the entity corresponding to a given index in the pipeline
-    pub fn entity(&self, index: usize) -> Entity {
-        self.proxies[index].entity
-    }
+    fn update_internal(&mut self, proxies: Vec<BvhProxy>) {
+        self.bvh = build_bvh2(
+            &proxies,
+            BvhBuildParams::fastest_build(),
+            &mut core::time::Duration::default(),
+        );
 
-    /// Get a dyn reference to the query dispatcher used in this pipeline
-    pub fn dispatcher_ref(&self) -> &dyn QueryDispatcher {
-        self.dispatcher.as_ref()
+        self.proxies = proxies;
     }
 
     /// Casts a [ray](spatial_query#raycasting) and computes the closest [hit](RayHitData) with a collider.
@@ -150,101 +110,47 @@ impl SpatialQueryPipeline {
     /// - `origin`: Where the ray is cast from.
     /// - `direction`: What direction the ray is cast in.
     /// - `max_distance`: The maximum distance the ray can travel.
-    /// - `solid`: If true *and* the ray origin is inside of a collider, the hit point will be the ray origin itself.
-    ///   Otherwise, the collider will be treated as hollow, and the hit point will be at its boundary.
     /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
+    /// - `f`: A function called on each entity hit by the ray. The ray keeps travelling until a valid distance is returned.
     ///
     /// # Related Methods
     ///
-    /// - [`SpatialQueryPipeline::cast_ray_predicate`]
+    /// - [`SpatialQueryPipeline::cast_ray`]
     /// - [`SpatialQueryPipeline::ray_hits`]
-    /// - [`SpatialQueryPipeline::ray_hits_callback`]
     pub fn cast_ray(
         &self,
         origin: Vector,
         direction: Dir,
         max_distance: Scalar,
-        solid: bool,
         filter: &SpatialQueryFilter,
-    ) -> Option<RayHitData> {
-        self.cast_ray_predicate(origin, direction, max_distance, solid, filter, &|_| true)
-    }
+        mut f: impl FnMut(Entity, obvhs::ray::Ray) -> f32,
+    ) -> Option<(Entity, f32)> {
+        if self.bvh.nodes.is_empty() {
+            return None;
+        }
 
-    /// Casts a [ray](spatial_query#raycasting) and computes the closest [hit](RayHitData) with a collider.
-    /// If there are no hits, `None` is returned.
-    ///
-    /// # Arguments
-    ///
-    /// - `origin`: Where the ray is cast from.
-    /// - `direction`: What direction the ray is cast in.
-    /// - `max_distance`: The maximum distance the ray can travel.
-    /// - `solid`: If true *and* the ray origin is inside of a collider, the hit point will be the ray origin itself.
-    ///   Otherwise, the collider will be treated as hollow, and the hit point will be at its boundary.
-    /// - `predicate`: A function called on each entity hit by the ray. The ray keeps travelling until the predicate returns `true`.
-    /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
-    ///
-    /// # Related Methods
-    ///
-    /// - [`SpatialQueryPipeline::cast_ray`]
-    /// - [`SpatialQueryPipeline::ray_hits`]
-    /// - [`SpatialQueryPipeline::ray_hits_callback`]
-    pub fn cast_ray_predicate(
-        &self,
-        origin: Vector,
-        direction: Dir,
-        max_distance: Scalar,
-        solid: bool,
-        filter: &SpatialQueryFilter,
-        predicate: &dyn Fn(Entity) -> bool,
-    ) -> Option<RayHitData> {
-        let composite = self.as_composite_shape_with_predicate(filter, predicate);
-        let pipeline_shape = CompositeShapeRef(&composite);
-        let ray = parry::query::Ray::new(origin.into(), direction.adjust_precision().into());
-
-        pipeline_shape
-            .cast_local_ray_and_get_normal(&ray, max_distance, solid)
-            .map(|(index, hit)| RayHitData {
-                entity: self.proxies[index as usize].entity,
-                distance: hit.time_of_impact,
-                normal: hit.normal.into(),
-            })
-    }
+        #[cfg(feature = "2d")]
+        let origin = origin.extend(0.);
+        #[cfg(feature = "2d")]
+        let direction = Dir3::new_unchecked(direction.extend(0.));
+        let ray = obvhs::ray::Ray::new(origin.into(), (*direction).into(), 0., max_distance);
+
+        let mut hit = RayHit::none();
+        if !self.bvh.ray_traverse(ray, &mut hit, |&ray, idx| {
+            let proxy = self.get_proxy(idx);
+            if proxy.layers.memberships & filter.mask == LayerMask::NONE
+                || filter.excluded_entities.contains(&proxy.entity)
+            {
+                return f32::INFINITY;
+            }
 
-    /// Casts a [ray](spatial_query#raycasting) and computes all [hits](RayHitData) until `max_hits` is reached.
-    ///
-    /// Note that the order of the results is not guaranteed, and if there are more hits than `max_hits`,
-    /// some hits will be missed.
-    ///
-    /// # Arguments
-    ///
-    /// - `origin`: Where the ray is cast from.
-    /// - `direction`: What direction the ray is cast in.
-    /// - `max_distance`: The maximum distance the ray can travel.
-    /// - `max_hits`: The maximum number of hits. Additional hits will be missed.
-    /// - `solid`: If true *and* the ray origin is inside of a collider, the hit point will be the ray origin itself.
-    ///   Otherwise, the collider will be treated as hollow, and the hit point will be at its boundary.
-    /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
-    ///
-    /// # Related Methods
-    ///
-    /// - [`SpatialQueryPipeline::cast_ray`]
-    /// - [`SpatialQueryPipeline::cast_ray_predicate`]
-    /// - [`SpatialQueryPipeline::ray_hits_callback`]
-    pub fn ray_hits(
-        &self,
-        origin: Vector,
-        direction: Dir,
-        max_distance: Scalar,
-        max_hits: u32,
-        solid: bool,
-        filter: &SpatialQueryFilter,
-    ) -> Vec<RayHitData> {
-        let mut hits = Vec::with_capacity(10);
-        self.ray_hits_callback(origin, direction, max_distance, solid, filter, |hit| {
-            hits.push(hit);
-            (hits.len() as u32) < max_hits
-        });
-        hits
+            f(proxy.entity, ray)
+        }) {
+            return None;
+        }
+
+        let entity = self.get_proxy(hit.primitive_id as usize).entity;
+        Some((entity, hit.t))
     }
 
     /// Casts a [ray](spatial_query#raycasting) and computes all [hits](RayHitData), calling the given `callback`
@@ -257,8 +163,6 @@ impl SpatialQueryPipeline {
     /// - `origin`: Where the ray is cast from.
     /// - `direction`: What direction the ray is cast in.
     /// - `max_distance`: The maximum distance the ray can travel.
-    /// - `solid`: If true *and* the ray origin is inside of a collider, the hit point will be the ray origin itself.
-    ///   Otherwise, the collider will be treated as hollow, and the hit point will be at its boundary.
     /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
     /// - `callback`: A callback function called for each hit.
     ///
@@ -266,50 +170,42 @@ impl SpatialQueryPipeline {
     ///
     /// - [`SpatialQueryPipeline::cast_ray`]
     /// - [`SpatialQueryPipeline::cast_ray_predicate`]
-    /// - [`SpatialQueryPipeline::ray_hits`]
-    pub fn ray_hits_callback(
+    pub fn ray_hits(
         &self,
         origin: Vector,
         direction: Dir,
         max_distance: Scalar,
-        solid: bool,
         filter: &SpatialQueryFilter,
-        // TODO: Just return an iterator
-        mut callback: impl FnMut(RayHitData) -> bool,
+        mut f: impl FnMut(Entity, obvhs::ray::Ray) -> bool,
     ) {
-        let proxies = &self.proxies;
-
-        let ray = parry::query::Ray::new(origin.into(), direction.adjust_precision().into());
-
-        let hits = self
-            .bvh
-            .leaves(move |node: &BvhNode| node.aabb().intersects_local_ray(&ray, max_distance))
-            .filter_map(move |leaf| {
-                let proxy = proxies.get(leaf as usize)?;
-
-                if !filter.test(proxy.entity, proxy.layers) {
-                    return None;
-                }
-
-                let hit = proxy.collider.shape_scaled().cast_ray_and_get_normal(
-                    &proxy.isometry,
-                    &ray,
-                    max_distance,
-                    solid,
-                )?;
+        if self.bvh.nodes.is_empty() {
+            return;
+        }
 
-                Some(RayHitData {
-                    entity: proxy.entity,
-                    distance: hit.time_of_impact,
-                    normal: hit.normal.into(),
+        #[cfg(feature = "2d")]
+        let origin = origin.extend(0.);
+        #[cfg(feature = "2d")]
+        let direction = Dir3::new_unchecked(direction.extend(0.));
+        let ray = obvhs::ray::Ray::new(origin.into(), (*direction).into(), 0., max_distance);
+
+        let mut traversal = self.bvh.new_ray_traversal(ray);
+        let mut hit = RayHit::none();
+        let mut c = true;
+        while c
+            && self
+                .bvh
+                .ray_traverse_dynamic(&mut traversal, &mut hit, |&ray, idx| {
+                    let proxy = self.get_proxy(idx);
+                    if proxy.layers.memberships & filter.mask == LayerMask::NONE
+                        || filter.excluded_entities.contains(&proxy.entity)
+                    {
+                        return f32::INFINITY;
+                    }
+
+                    c = f(proxy.entity, ray);
+                    return f32::INFINITY;
                 })
-            });
-
-        for hit in hits {
-            if !callback(hit) {
-                break;
-            }
-        }
+        {}
     }
 
     /// Casts a [shape](spatial_query#shapecasting) with a given rotation and computes the closest [hit](ShapeHits)
@@ -332,24 +228,63 @@ impl SpatialQueryPipeline {
     /// - [`SpatialQueryPipeline::shape_hits`]
     /// - [`SpatialQueryPipeline::shape_hits_callback`]
     #[allow(clippy::too_many_arguments)]
-    pub fn cast_shape(
+    pub fn cast_aabb(
         &self,
-        shape: &Collider,
+        shape: ColliderAabb,
         origin: Vector,
-        shape_rotation: RotationValue,
         direction: Dir,
         config: &ShapeCastConfig,
         filter: &SpatialQueryFilter,
-    ) -> Option<ShapeHitData> {
-        self.cast_shape_predicate(
-            shape,
-            origin,
-            shape_rotation,
-            direction,
-            config,
-            filter,
-            &|_| true,
-        )
+        mut f: impl FnMut(Entity, obvhs::ray::Ray) -> Option<QueryShapeCastHit>,
+    ) -> Option<ShapeCastHit> {
+        if self.bvh.nodes.is_empty() {
+            return None;
+        }
+
+        #[cfg(feature = "2d")]
+        let shape = Aabb {
+            min: shape.min.extend(0.).into(),
+            max: shape.max.extend(0.).into(),
+        };
+        #[cfg(feature = "3d")]
+        let shape = Aabb {
+            min: shape.min.into(),
+            max: shape.max.into(),
+        };
+
+        #[cfg(feature = "2d")]
+        let origin = origin.extend(0.);
+        #[cfg(feature = "2d")]
+        let direction = Dir3::new_unchecked(direction.extend(0.));
+        let ray = obvhs::ray::Ray::new(origin.into(), (*direction).into(), 0., config.max_distance);
+
+        let mut hit = RayHit::none();
+        let mut shape_hit = None::<ShapeCastHit>;
+        if !self.bvh.cast_traverse(ray, &mut hit, shape, |&ray, idx| {
+            let proxy = self.get_proxy(idx);
+
+            if proxy.layers.memberships & filter.mask == LayerMask::NONE
+                || filter.excluded_entities.contains(&proxy.entity)
+            {
+                return f32::INFINITY;
+            }
+
+            let hit = f(proxy.entity, ray);
+            if let Some(hit) = hit {
+                shape_hit = Some(ShapeCastHit {
+                    entity: proxy.entity,
+                    distance: hit.distance,
+                    point: hit.point,
+                    normal: hit.normal,
+                });
+            }
+
+            hit.map(|hit| hit.distance).unwrap_or(f32::INFINITY)
+        }) {
+            return None;
+        }
+
+        shape_hit
     }
 
     /// Casts a [shape](spatial_query#shapecasting) with a given rotation and computes the closest [hit](ShapeHits)
@@ -373,185 +308,54 @@ impl SpatialQueryPipeline {
     /// - [`SpatialQueryPipeline::shape_hits`]
     /// - [`SpatialQueryPipeline::shape_hits_callback`]
     #[allow(clippy::too_many_arguments)]
-    pub fn cast_shape_predicate(
-        &self,
-        shape: &Collider,
-        origin: Vector,
-        shape_rotation: RotationValue,
-        direction: Dir,
-        config: &ShapeCastConfig,
-        filter: &SpatialQueryFilter,
-        predicate: &dyn Fn(Entity) -> bool,
-    ) -> Option<ShapeHitData> {
-        let rotation: Rotation;
-        #[cfg(feature = "2d")]
-        {
-            rotation = Rotation::radians(shape_rotation);
-        }
-        #[cfg(feature = "3d")]
-        {
-            rotation = Rotation::from(shape_rotation);
-        }
-
-        let shape_isometry = make_isometry(origin, rotation);
-        let shape_direction = direction.adjust_precision().into();
-        let composite = self.as_composite_shape_with_predicate(filter, predicate);
-        let pipeline_shape = CompositeShapeRef(&composite);
-
-        pipeline_shape
-            .cast_shape(
-                self.dispatcher.as_ref(),
-                &shape_isometry,
-                &shape_direction,
-                shape.shape_scaled().as_ref(),
-                ShapeCastOptions {
-                    max_time_of_impact: config.max_distance,
-                    stop_at_penetration: !config.ignore_origin_penetration,
-                    compute_impact_geometry_on_penetration: config.compute_contact_on_penetration,
-                    ..default()
-                },
-            )
-            .map(|(index, hit)| ShapeHitData {
-                entity: self.proxies[index as usize].entity,
-                distance: hit.time_of_impact,
-                point1: hit.witness1.into(),
-                point2: hit.witness2.into(),
-                normal1: hit.normal1.into(),
-                normal2: hit.normal2.into(),
-            })
-    }
-
-    /// Casts a [shape](spatial_query#shapecasting) with a given rotation and computes computes all [hits](ShapeHitData)
-    /// in the order of distance until `max_hits` is reached.
-    ///
-    /// # Arguments
-    ///
-    /// - `shape`: The shape being cast represented as a [`Collider`].
-    /// - `origin`: Where the shape is cast from.
-    /// - `shape_rotation`: The rotation of the shape being cast.
-    /// - `direction`: What direction the shape is cast in.
-    /// - `max_hits`: The maximum number of hits. Additional hits will be missed.
-    /// - `config`: A [`ShapeCastConfig`] that determines the behavior of the cast.
-    /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
-    ///
-    /// # Related Methods
-    ///
-    /// - [`SpatialQueryPipeline::cast_shape`]
-    /// - [`SpatialQueryPipeline::cast_shape_predicate`]
-    /// - [`SpatialQueryPipeline::shape_hits_callback`]
-    #[allow(clippy::too_many_arguments)]
     pub fn shape_hits(
         &self,
-        shape: &Collider,
-        origin: Vector,
-        shape_rotation: RotationValue,
-        direction: Dir,
-        max_hits: u32,
-        config: &ShapeCastConfig,
-        filter: &SpatialQueryFilter,
-    ) -> Vec<ShapeHitData> {
-        let mut hits = Vec::with_capacity(10);
-        self.shape_hits_callback(
-            shape,
-            origin,
-            shape_rotation,
-            direction,
-            config,
-            filter,
-            |hit| {
-                hits.push(hit);
-                (hits.len() as u32) < max_hits
-            },
-        );
-        hits
-    }
-
-    /// Casts a [shape](spatial_query#shapecasting) with a given rotation and computes computes all [hits](ShapeHitData)
-    /// in the order of distance, calling the given `callback` for each hit. The shapecast stops when
-    /// `callback` returns false or all hits have been found.
-    ///
-    /// # Arguments
-    ///
-    /// - `shape`: The shape being cast represented as a [`Collider`].
-    /// - `origin`: Where the shape is cast from.
-    /// - `shape_rotation`: The rotation of the shape being cast.
-    /// - `direction`: What direction the shape is cast in.
-    /// - `config`: A [`ShapeCastConfig`] that determines the behavior of the cast.
-    /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
-    /// - `callback`: A callback function called for each hit.
-    ///
-    /// # Related Methods
-    ///
-    /// - [`SpatialQueryPipeline::cast_shape`]
-    /// - [`SpatialQueryPipeline::cast_shape_predicate`]
-    /// - [`SpatialQueryPipeline::shape_hits`]
-    #[allow(clippy::too_many_arguments)]
-    pub fn shape_hits_callback(
-        &self,
-        shape: &Collider,
+        shape: ColliderAabb,
         origin: Vector,
-        shape_rotation: RotationValue,
         direction: Dir,
         config: &ShapeCastConfig,
         filter: &SpatialQueryFilter,
-        mut callback: impl FnMut(ShapeHitData) -> bool,
+        mut f: impl FnMut(Entity, obvhs::ray::Ray) -> bool,
     ) {
-        // TODO: This clone is here so that the excluded entities in the original `query_filter` aren't modified.
-        //       We could remove this if shapecasting could compute multiple hits without just doing casts in a loop.
-        //       See https://github.com/avianphysics/avian/issues/403.
-        let mut query_filter = filter.clone();
-
-        let shape_cast_options = ShapeCastOptions {
-            max_time_of_impact: config.max_distance,
-            target_distance: config.target_distance,
-            stop_at_penetration: !config.ignore_origin_penetration,
-            compute_impact_geometry_on_penetration: config.compute_contact_on_penetration,
-        };
+        if self.bvh.nodes.is_empty() {
+            return;
+        }
 
-        let rotation: Rotation;
         #[cfg(feature = "2d")]
-        {
-            rotation = Rotation::radians(shape_rotation);
-        }
+        let shape = Aabb {
+            min: shape.min.extend(0.).into(),
+            max: shape.max.extend(0.).into(),
+        };
         #[cfg(feature = "3d")]
-        {
-            rotation = Rotation::from(shape_rotation);
-        }
-
-        let shape_isometry = make_isometry(origin, rotation);
-        let shape_direction = direction.adjust_precision().into();
-
-        loop {
-            let composite = self.as_composite_shape_internal(&query_filter);
-            let pipeline_shape = CompositeShapeRef(&composite);
-
-            let hit = pipeline_shape
-                .cast_shape(
-                    self.dispatcher.as_ref(),
-                    &shape_isometry,
-                    &shape_direction,
-                    shape.shape_scaled().as_ref(),
-                    shape_cast_options,
-                )
-                .map(|(index, hit)| ShapeHitData {
-                    entity: self.proxies[index as usize].entity,
-                    distance: hit.time_of_impact,
-                    point1: hit.witness1.into(),
-                    point2: hit.witness2.into(),
-                    normal1: hit.normal1.into(),
-                    normal2: hit.normal2.into(),
-                });
-
-            if let Some(hit) = hit {
-                query_filter.excluded_entities.insert(hit.entity);
+        let shape = Aabb {
+            min: shape.min.into(),
+            max: shape.max.into(),
+        };
 
-                if !callback(hit) {
-                    break;
-                }
-            } else {
-                break;
-            }
-        }
+        #[cfg(feature = "2d")]
+        let origin = origin.extend(0.);
+        #[cfg(feature = "2d")]
+        let direction = Dir3::new_unchecked(direction.extend(0.));
+        let ray = obvhs::ray::Ray::new(origin.into(), (*direction).into(), 0., config.max_distance);
+
+        let mut traversal = self.bvh.new_ray_traversal(ray);
+        let mut hit = RayHit::none();
+        let mut c = true;
+        while c
+            && self
+                .bvh
+                .cast_traverse_dynamic(&mut traversal, &mut hit, shape, |&ray, idx| {
+                    let proxy = self.get_proxy(idx);
+                    if proxy.layers.memberships & filter.mask == LayerMask::NONE
+                        || filter.excluded_entities.contains(&proxy.entity)
+                    {
+                        return f32::INFINITY;
+                    }
+
+                    c = f(proxy.entity, ray);
+                    f32::INFINITY
+                })
+        {}
     }
 
     /// Finds the [projection](spatial_query#point-projection) of a given point on the closest [collider](Collider).
@@ -570,116 +374,98 @@ impl SpatialQueryPipeline {
     pub fn project_point(
         &self,
         point: Vector,
-        solid: bool,
         filter: &SpatialQueryFilter,
+        mut f: impl FnMut(Entity, Vector) -> f32,
     ) -> Option<PointProjection> {
-        self.project_point_predicate(point, solid, filter, &|_| true)
-    }
-
-    /// Finds the [projection](spatial_query#point-projection) of a given point on the closest [collider](Collider).
-    /// If one isn't found, `None` is returned.
-    ///
-    /// # Arguments
-    ///
-    /// - `point`: The point that should be projected.
-    /// - `solid`: If true and the point is inside of a collider, the projection will be at the point.
-    ///   Otherwise, the collider will be treated as hollow, and the projection will be at the collider's boundary.
-    /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
-    /// - `predicate`: A function for filtering which entities are considered in the query. The projection will be on the closest collider for which the `predicate` returns `true`
-    ///
-    /// # Related Methods
-    ///
-    /// - [`SpatialQueryPipeline::project_point`]
-    pub fn project_point_predicate(
-        &self,
-        point: Vector,
-        solid: bool,
-        filter: &SpatialQueryFilter,
-        predicate: &dyn Fn(Entity) -> bool,
-    ) -> Option<PointProjection> {
-        if self.proxies.is_empty() {
+        if self.bvh.nodes.is_empty() {
             return None;
         }
 
+        #[cfg(feature = "2d")]
+        let point = point.extend(0.).into();
+        #[cfg(feature = "3d")]
         let point = point.into();
-        let composite = self.as_composite_shape_with_predicate(filter, predicate);
-        let pipeline_shape = CompositeShapeRef(&composite);
 
-        let (index, projection) = pipeline_shape.project_local_point(&point, solid);
+        let mut hit = RayHit::none();
+        self.bvh.dist_traverse(point, &mut hit, |pos, idx| {
+            let proxy = self.get_proxy(idx);
 
-        Some(PointProjection {
-            entity: self.proxies[index as usize].entity,
-            point: projection.point.into(),
-            is_inside: projection.is_inside,
-        })
-    }
+            if proxy.layers.memberships & filter.mask == LayerMask::NONE
+                || filter.excluded_entities.contains(&proxy.entity)
+            {
+                return f32::INFINITY;
+            }
 
-    /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [collider](Collider)
-    /// that contains the given point.
-    ///
-    /// # Arguments
-    ///
-    /// - `point`: The point that intersections are tested against.
-    /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
-    ///
-    /// # Related Methods
-    ///
-    /// - [`SpatialQueryPipeline::point_intersections_callback`]
-    pub fn point_intersections(&self, point: Vector, filter: &SpatialQueryFilter) -> Vec<Entity> {
-        let mut intersections = vec![];
+            #[cfg(feature = "2d")]
+            let pos = pos.xy();
+            #[cfg(feature = "3d")]
+            let pos = (*pos).into();
 
-        self.point_intersections_callback(point, filter, |e| {
-            intersections.push(e);
-            true
+            f(proxy.entity, pos)
         });
 
-        intersections
+        if hit.t == f32::INFINITY {
+            return None;
+        }
+
+        let entity = self.get_proxy(hit.primitive_id as usize).entity;
+        Some(PointProjection {
+            entity,
+            is_inside: hit.t.is_sign_negative(),
+        })
     }
 
     /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [collider](Collider)
-    /// that contains the given point, calling the given `callback` for each intersection.
-    /// The search stops when `callback` returns `false` or all intersections have been found.
+    /// that contains the given point.
     ///
     /// # Arguments
     ///
     /// - `point`: The point that intersections are tested against.
     /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
-    /// - `callback`: A callback function called for each intersection.
     ///
     /// # Related Methods
     ///
-    /// - [`SpatialQueryPipeline::point_intersections`]
-    pub fn point_intersections_callback(
+    /// - [`SpatialQueryPipeline::point_intersections_callback`]
+    pub fn point_intersections(
         &self,
         point: Vector,
         filter: &SpatialQueryFilter,
-        mut callback: impl FnMut(Entity) -> bool,
+        mut f: impl FnMut(Entity, Vector) -> (bool, bool),
     ) {
-        let point = point.into();
-
-        let intersecting_entities = self
-            .bvh
-            .leaves(|node: &BvhNode| node.aabb().contains_local_point(&point))
-            .filter_map(move |leaf| {
-                let proxy = self.proxies.get(leaf as usize)?;
-
-                if filter.test(proxy.entity, proxy.layers)
-                    && proxy
-                        .collider
-                        .shape_scaled()
-                        .contains_point(&proxy.isometry, &point)
-                {
-                    Some(proxy.entity)
-                } else {
-                    None
-                }
-            });
-
-        for entity in intersecting_entities {
-            if !callback(entity) {
-                break;
-            }
+        if self.bvh.nodes.is_empty() {
+            return;
         }
+
+        #[cfg(feature = "2d")]
+        let pos = point.extend(0.).into();
+        #[cfg(feature = "3d")]
+        let pos = point.into();
+
+        let mut state = PointTraversal::new(&self.bvh, pos);
+        let mut hit = RayHit::none();
+        let mut c = true;
+        while c
+            && self
+                .bvh
+                .point_traverse_dynamic(&mut state, &mut hit, |pos, idx| {
+                    let proxy = self.get_proxy(idx);
+
+                    if proxy.layers.memberships & filter.mask == LayerMask::NONE
+                        || filter.excluded_entities.contains(&proxy.entity)
+                    {
+                        return false;
+                    }
+
+                    #[cfg(feature = "2d")]
+                    let pos = pos.xy();
+                    #[cfg(feature = "3d")]
+                    let pos = (*pos).into();
+
+                    let hit: bool;
+                    (hit, c) = f(proxy.entity, pos);
+                    hit
+                })
+        {}
     }
 
     /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [`ColliderAabb`]
@@ -688,272 +474,44 @@ impl SpatialQueryPipeline {
     /// # Related Methods
     ///
     /// - [`SpatialQueryPipeline::aabb_intersections_with_aabb_callback`]
-    pub fn aabb_intersections_with_aabb(&self, aabb: ColliderAabb) -> Vec<Entity> {
-        let mut intersections = vec![];
-
-        self.aabb_intersections_with_aabb_callback(aabb, |e| {
-            intersections.push(e);
-            true
-        });
-
-        intersections
-    }
-
-    /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [`ColliderAabb`]
-    /// that is intersecting the given `aabb`, calling `callback` for each intersection.
-    /// The search stops when `callback` returns `false` or all intersections have been found.
-    ///
-    /// # Related Methods
-    ///
-    /// - [`SpatialQueryPipeline::aabb_intersections_with_aabb`]
-    pub fn aabb_intersections_with_aabb_callback(
+    pub fn aabb_intersections_with_aabb(
         &self,
         aabb: ColliderAabb,
-        mut callback: impl FnMut(Entity) -> bool,
+        filter: &SpatialQueryFilter,
+        mut f: impl FnMut(Entity) -> bool,
     ) {
-        let aabb = Aabb {
-            mins: aabb.min.into(),
-            maxs: aabb.max.into(),
-        };
-
-        let intersecting_entities = self
-            .bvh
-            .leaves(move |node: &BvhNode| node.aabb().intersects(&aabb))
-            .filter_map(move |leaf| self.proxies.get(leaf as usize).map(|p| p.entity));
-
-        for entity in intersecting_entities {
-            if !callback(entity) {
-                break;
-            }
+        if self.bvh.nodes.is_empty() {
+            return;
         }
-    }
 
-    /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [`Collider`]
-    /// that is intersecting the given `shape` with a given position and rotation.
-    ///
-    /// # Arguments
-    ///
-    /// - `shape`: The shape that intersections are tested against represented as a [`Collider`].
-    /// - `shape_position`: The position of the shape.
-    /// - `shape_rotation`: The rotation of the shape.
-    /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
-    ///
-    /// # Related Methods
-    ///
-    /// - [`SpatialQueryPipeline::shape_intersections_callback`]
-    pub fn shape_intersections(
-        &self,
-        shape: &Collider,
-        shape_position: Vector,
-        shape_rotation: RotationValue,
-        filter: &SpatialQueryFilter,
-    ) -> Vec<Entity> {
-        let mut intersections = vec![];
-        self.shape_intersections_callback(shape, shape_position, shape_rotation, filter, |e| {
-            intersections.push(e);
-            true
-        });
-        intersections
-    }
-
-    /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [`Collider`]
-    /// that is intersecting the given `shape` with a given position and rotation, calling `callback` for each
-    /// intersection. The search stops when `callback` returns `false` or all intersections have been found.
-    ///
-    /// # Arguments
-    ///
-    /// - `shape`: The shape that intersections are tested against represented as a [`Collider`].
-    /// - `shape_position`: The position of the shape.
-    /// - `shape_rotation`: The rotation of the shape.
-    /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
-    /// - `callback`: A callback function called for each intersection.
-    ///
-    /// # Related Methods
-    ///
-    /// - [`SpatialQueryPipeline::shape_intersections`]
-    pub fn shape_intersections_callback(
-        &self,
-        shape: &Collider,
-        shape_position: Vector,
-        shape_rotation: RotationValue,
-        filter: &SpatialQueryFilter,
-        mut callback: impl FnMut(Entity) -> bool,
-    ) {
-        let proxies = &self.proxies;
-        let rotation: Rotation;
         #[cfg(feature = "2d")]
-        {
-            rotation = Rotation::radians(shape_rotation);
-        }
+        let aabb = Aabb {
+            min: aabb.min.extend(0.).into(),
+            max: aabb.max.extend(0.).into(),
+        };
         #[cfg(feature = "3d")]
-        {
-            rotation = Rotation::from(shape_rotation);
-        }
-
-        let shape_isometry = make_isometry(shape_position, rotation);
-        let inverse_shape_isometry = shape_isometry.inverse();
-
-        let dispatcher = &*self.dispatcher;
-
-        let shape_aabb = shape.shape_scaled().compute_aabb(&shape_isometry);
-        let entities = self
-            .bvh
-            .leaves(move |node: &BvhNode| node.aabb().intersects(&shape_aabb))
-            .filter_map(move |leaf| {
-                let proxy = proxies.get(leaf as usize)?;
+        let aabb = Aabb {
+            min: aabb.min.into(),
+            max: aabb.max.into(),
+        };
 
-                if !filter.test(proxy.entity, proxy.layers) {
-                    return None;
+        self.bvh.aabb_traverse(aabb, |_, idx| {
+            let first = self.bvh.nodes[idx as usize].first_index;
+            let n = self.bvh.nodes[idx as usize].prim_count;
+            for idx in first..(first + n) {
+                let proxy = self.get_proxy(idx as usize);
+                if proxy.layers.memberships & filter.mask == LayerMask::NONE
+                    || filter.excluded_entities.contains(&proxy.entity)
+                {
+                    continue;
                 }
 
-                let isometry = inverse_shape_isometry * proxy.isometry;
-                let intersects = dispatcher
-                    .intersection_test(
-                        &isometry,
-                        shape.shape_scaled().as_ref(),
-                        proxy.collider.shape_scaled().as_ref(),
-                    )
-                    .is_ok_and(|b| b);
-
-                intersects.then_some(proxy.entity)
-            });
-
-        for entity in entities {
-            if !callback(entity) {
-                break;
+                if !f(proxy.entity) {
+                    return false;
+                }
             }
-        }
-    }
-}
-
-pub(crate) struct QueryPipelineAsCompositeShape<'a> {
-    pipeline: &'a SpatialQueryPipeline,
-    query_filter: &'a SpatialQueryFilter,
-}
-
-impl CompositeShape for QueryPipelineAsCompositeShape<'_> {
-    fn map_part_at(
-        &self,
-        shape_id: u32,
-        f: &mut dyn FnMut(Option<&Isometry<Scalar>>, &dyn Shape, Option<&dyn NormalConstraints>),
-    ) {
-        self.map_untyped_part_at(shape_id, f);
-    }
-
-    fn bvh(&self) -> &Bvh {
-        &self.pipeline.bvh
-    }
-}
-
-impl TypedCompositeShape for QueryPipelineAsCompositeShape<'_> {
-    type PartNormalConstraints = ();
-    type PartShape = dyn Shape;
-
-    fn map_typed_part_at<T>(
-        &self,
-        shape_id: u32,
-        mut f: impl FnMut(
-            Option<&Isometry<Scalar>>,
-            &Self::PartShape,
-            Option<&Self::PartNormalConstraints>,
-        ) -> T,
-    ) -> Option<T> {
-        let proxy = self.pipeline.proxies.get(shape_id as usize)?;
-
-        if self.query_filter.test(proxy.entity, proxy.layers) {
-            Some(f(
-                Some(&proxy.isometry),
-                proxy.collider.shape_scaled().as_ref(),
-                None,
-            ))
-        } else {
-            None
-        }
-    }
-
-    fn map_untyped_part_at<T>(
-        &self,
-        shape_id: u32,
-        mut f: impl FnMut(Option<&Isometry<Scalar>>, &dyn Shape, Option<&dyn NormalConstraints>) -> T,
-    ) -> Option<T> {
-        let proxy = self.pipeline.proxies.get(shape_id as usize)?;
-
-        if self.query_filter.test(proxy.entity, proxy.layers) {
-            Some(f(
-                Some(&proxy.isometry),
-                proxy.collider.shape_scaled().as_ref(),
-                None,
-            ))
-        } else {
-            None
-        }
-    }
-}
-
-pub(crate) struct QueryPipelineAsCompositeShapeWithPredicate<'a, 'b> {
-    pipeline: &'a SpatialQueryPipeline,
-    query_filter: &'a SpatialQueryFilter,
-    predicate: &'b dyn Fn(Entity) -> bool,
-}
-
-impl CompositeShape for QueryPipelineAsCompositeShapeWithPredicate<'_, '_> {
-    fn map_part_at(
-        &self,
-        shape_id: u32,
-        f: &mut dyn FnMut(Option<&Isometry<Scalar>>, &dyn Shape, Option<&dyn NormalConstraints>),
-    ) {
-        self.map_untyped_part_at(shape_id, f);
-    }
-
-    fn bvh(&self) -> &Bvh {
-        &self.pipeline.bvh
-    }
-}
-
-impl TypedCompositeShape for QueryPipelineAsCompositeShapeWithPredicate<'_, '_> {
-    type PartNormalConstraints = ();
-    type PartShape = dyn Shape;
-
-    fn map_typed_part_at<T>(
-        &self,
-        shape_id: u32,
-        mut f: impl FnMut(
-            Option<&Isometry<Scalar>>,
-            &Self::PartShape,
-            Option<&Self::PartNormalConstraints>,
-        ) -> T,
-    ) -> Option<T> {
-        if let Some(proxy) = self.pipeline.proxies.get(shape_id as usize)
-            && self.query_filter.test(proxy.entity, proxy.layers)
-            && (self.predicate)(proxy.entity)
-        {
-            Some(f(
-                Some(&proxy.isometry),
-                proxy.collider.shape_scaled().as_ref(),
-                None,
-            ))
-        } else {
-            None
-        }
-    }
-
-    fn map_untyped_part_at<T>(
-        &self,
-        shape_id: u32,
-        mut f: impl FnMut(Option<&Isometry<Scalar>>, &dyn Shape, Option<&dyn NormalConstraints>) -> T,
-    ) -> Option<T> {
-        if let Some(proxy) = self.pipeline.proxies.get(shape_id as usize)
-            && self.query_filter.test(proxy.entity, proxy.layers)
-            && (self.predicate)(proxy.entity)
-        {
-            Some(f(
-                Some(&proxy.isometry),
-                proxy.collider.shape_scaled().as_ref(),
-                None,
-            ))
-        } else {
-            None
-        }
+            true
+        })
     }
 }
 
@@ -965,8 +523,6 @@ impl TypedCompositeShape for QueryPipelineAsCompositeShapeWithPredicate<'_, '_>
 pub struct PointProjection {
     /// The entity of the collider that the point was projected onto.
     pub entity: Entity,
-    /// The point where the point was projected.
-    pub point: Vector,
     /// True if the point was inside of the collider.
     pub is_inside: bool,
 }
diff --git a/src/spatial_query/ray_caster.rs b/src/spatial_query/ray_caster.rs
index b949dc38..98869bc8 100644
--- a/src/spatial_query/ray_caster.rs
+++ b/src/spatial_query/ray_caster.rs
@@ -7,11 +7,6 @@ use bevy::{
     },
     prelude::*,
 };
-#[cfg(all(
-    feature = "default-collider",
-    any(feature = "parry-f32", feature = "parry-f64")
-))]
-use parry::{partitioning::BvhNode, query::RayCast};
 
 /// A component used for [raycasting](spatial_query#raycasting).
 ///
@@ -243,10 +238,6 @@ impl RayCaster {
         self.global_direction = global_direction;
     }
 
-    #[cfg(all(
-        feature = "default-collider",
-        any(feature = "parry-f32", feature = "parry-f64")
-    ))]
     pub(crate) fn cast(
         &mut self,
         caster_entity: Entity,
@@ -261,51 +252,9 @@ impl RayCaster {
 
         hits.clear();
 
-        if self.max_hits == 1 {
-            let first_hit = query_pipeline.cast_ray(
-                self.global_origin(),
-                self.global_direction(),
-                self.max_distance,
-                self.solid,
-                &self.query_filter,
-            );
-
-            if let Some(hit) = first_hit {
-                hits.push(hit);
-            }
-        } else {
-            let ray = parry::query::Ray::new(
-                self.global_origin().into(),
-                self.global_direction().adjust_precision().into(),
-            );
-
-            let found_hits = query_pipeline
-                .bvh
-                .leaves(|node: &BvhNode| node.aabb().intersects_local_ray(&ray, self.max_distance))
-                .filter_map(|leaf| {
-                    let proxy = query_pipeline.proxies.get(leaf as usize)?;
-
-                    if !self.query_filter.test(proxy.entity, proxy.layers) {
-                        return None;
-                    }
-
-                    let hit = proxy.collider.shape_scaled().cast_ray_and_get_normal(
-                        &proxy.isometry,
-                        &ray,
-                        self.max_distance,
-                        self.solid,
-                    )?;
-
-                    Some(RayHitData {
-                        entity: proxy.entity,
-                        distance: hit.time_of_impact,
-                        normal: hit.normal.into(),
-                    })
-                })
-                .take(self.max_hits as usize);
-
-            hits.extend(found_hits);
-        }
+        // TODO
+        _ = query_pipeline;
+        todo!()
     }
 }
 
@@ -355,13 +304,13 @@ fn on_add_ray_caster(mut world: DeferredWorld, ctx: HookContext) {
 #[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
 #[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))]
 #[reflect(Component, Debug, Default, PartialEq)]
-pub struct RayHits(pub Vec<RayHitData>);
+pub struct RayHits(pub Vec<RayCastHit>);
 
 impl RayHits {
     /// Returns an iterator over the hits, sorted in ascending order according to the distance.
     ///
     /// Note that this allocates a new vector. If you don't need the hits in order, use `iter`.
-    pub fn iter_sorted(&self) -> alloc::vec::IntoIter<RayHitData> {
+    pub fn iter_sorted(&self) -> alloc::vec::IntoIter<RayCastHit> {
         let mut vector = self.as_slice().to_vec();
         vector.sort_by(|a, b| a.distance.partial_cmp(&b.distance).unwrap());
         vector.into_iter()
@@ -369,8 +318,8 @@ impl RayHits {
 }
 
 impl IntoIterator for RayHits {
-    type Item = RayHitData;
-    type IntoIter = alloc::vec::IntoIter<RayHitData>;
+    type Item = RayCastHit;
+    type IntoIter = alloc::vec::IntoIter<RayCastHit>;
 
     fn into_iter(self) -> Self::IntoIter {
         self.0.into_iter()
@@ -378,8 +327,8 @@ impl IntoIterator for RayHits {
 }
 
 impl<'a> IntoIterator for &'a RayHits {
-    type Item = &'a RayHitData;
-    type IntoIter = core::slice::Iter<'a, RayHitData>;
+    type Item = &'a RayCastHit;
+    type IntoIter = core::slice::Iter<'a, RayCastHit>;
 
     fn into_iter(self) -> Self::IntoIter {
         self.0.iter()
@@ -387,8 +336,8 @@ impl<'a> IntoIterator for &'a RayHits {
 }
 
 impl<'a> IntoIterator for &'a mut RayHits {
-    type Item = &'a mut RayHitData;
-    type IntoIter = core::slice::IterMut<'a, RayHitData>;
+    type Item = &'a mut RayCastHit;
+    type IntoIter = core::slice::IterMut<'a, RayCastHit>;
 
     fn into_iter(self) -> Self::IntoIter {
         self.0.iter_mut()
@@ -408,7 +357,7 @@ impl MapEntities for RayHits {
 #[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
 #[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))]
 #[reflect(Debug, PartialEq)]
-pub struct RayHitData {
+pub struct RayCastHit {
     /// The entity of the collider that was hit by the ray.
     pub entity: Entity,
 
@@ -419,7 +368,7 @@ pub struct RayHitData {
     pub normal: Vector,
 }
 
-impl MapEntities for RayHitData {
+impl MapEntities for RayCastHit {
     fn map_entities<M: EntityMapper>(&mut self, entity_mapper: &mut M) {
         self.entity = entity_mapper.get_mapped(self.entity);
     }
diff --git a/src/spatial_query/shape_caster.rs b/src/spatial_query/shape_caster.rs
index 78a753d6..3e70e729 100644
--- a/src/spatial_query/shape_caster.rs
+++ b/src/spatial_query/shape_caster.rs
@@ -1,4 +1,4 @@
-use crate::prelude::*;
+use crate::{collision::collider::QueryCollider, prelude::*};
 use bevy::{
     ecs::{
         entity::{EntityMapper, MapEntities},
@@ -7,7 +7,6 @@ use bevy::{
     },
     prelude::*,
 };
-use parry::{query::ShapeCastOptions, shape::CompositeShapeRef};
 
 /// A component used for [shapecasting](spatial_query#shapecasting).
 ///
@@ -55,18 +54,19 @@ use parry::{query::ShapeCastOptions, shape::CompositeShapeRef};
 /// }
 /// ```
 #[derive(Component, Clone, Debug, Reflect)]
-#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
-#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))]
-#[reflect(Debug, Component)]
-#[component(on_add = on_add_shape_caster)]
+// #[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
+// #[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))]
+#[reflect(Component)]
+#[reflect(from_reflect = false)]
+#[component(on_add = on_add_shape_caster::<C>)]
 #[require(ShapeHits)]
-pub struct ShapeCaster {
+pub struct ShapeCaster<C: QueryCollider> {
     /// Controls if the shape caster is enabled.
     pub enabled: bool,
 
-    /// The shape being cast represented as a [`Collider`].
+    /// The shape being cast represented as a defined for the [`QueryCollider`].
     #[reflect(ignore)]
-    pub shape: Collider,
+    pub shape: C::CastShape,
 
     /// The local origin of the shape relative to the [`Position`] and [`Rotation`]
     /// of the shape caster entity or its parent.
@@ -143,14 +143,11 @@ pub struct ShapeCaster {
     pub query_filter: SpatialQueryFilter,
 }
 
-impl Default for ShapeCaster {
+impl<C: QueryCollider> Default for ShapeCaster<C> {
     fn default() -> Self {
         Self {
             enabled: true,
-            #[cfg(feature = "2d")]
-            shape: Collider::circle(0.0),
-            #[cfg(feature = "3d")]
-            shape: Collider::sphere(0.0),
+            shape: C::CastShape::default(),
             origin: Vector::ZERO,
             global_origin: Vector::ZERO,
             #[cfg(feature = "2d")]
@@ -174,11 +171,11 @@ impl Default for ShapeCaster {
     }
 }
 
-impl ShapeCaster {
+impl<C: QueryCollider> ShapeCaster<C> {
     /// Creates a new [`ShapeCaster`] with a given shape, origin, shape rotation and direction.
     #[cfg(feature = "2d")]
     pub fn new(
-        shape: impl Into<Collider>,
+        shape: impl Into<C::CastShape>,
         origin: Vector,
         shape_rotation: Scalar,
         direction: Dir,
@@ -191,10 +188,11 @@ impl ShapeCaster {
             ..default()
         }
     }
+
     #[cfg(feature = "3d")]
     /// Creates a new [`ShapeCaster`] with a given shape, origin, shape rotation and direction.
     pub fn new(
-        shape: impl Into<Collider>,
+        shape: impl Into<C::CastShape>,
         origin: Vector,
         shape_rotation: Quaternion,
         direction: Dir,
@@ -338,24 +336,8 @@ impl ShapeCaster {
         hits: &mut ShapeHits,
         query_pipeline: &SpatialQueryPipeline,
     ) {
-        // TODO: This clone is here so that the excluded entities in the original `query_filter` aren't modified.
-        //       We could remove this if shapecasting could compute multiple hits without just doing casts in a loop.
-        //       See https://github.com/avianphysics/avian/issues/403.
-        let mut query_filter = self.query_filter.clone();
-
-        if self.ignore_self {
-            query_filter.excluded_entities.insert(caster_entity);
-        }
-
         hits.clear();
 
-        let shape_cast_options = ShapeCastOptions {
-            max_time_of_impact: self.max_distance,
-            target_distance: self.target_distance,
-            stop_at_penetration: !self.ignore_origin_penetration,
-            compute_impact_geometry_on_penetration: self.compute_contact_on_penetration,
-        };
-
         let shape_rotation: Rotation;
         #[cfg(feature = "2d")]
         {
@@ -366,42 +348,19 @@ impl ShapeCaster {
             shape_rotation = Rotation::from(self.global_shape_rotation());
         }
 
-        let shape_isometry = make_isometry(self.global_origin(), shape_rotation);
+        let shape_isometry = (self.global_origin(), shape_rotation);
         let shape_direction = self.global_direction().adjust_precision().into();
 
-        while hits.len() < self.max_hits as usize {
-            let composite = query_pipeline.as_composite_shape_internal(&query_filter);
-            let pipeline_shape = CompositeShapeRef(&composite);
-
-            let hit = pipeline_shape
-                .cast_shape(
-                    query_pipeline.dispatcher.as_ref(),
-                    &shape_isometry,
-                    &shape_direction,
-                    self.shape.shape_scaled().as_ref(),
-                    shape_cast_options,
-                )
-                .map(|(index, hit)| ShapeHitData {
-                    entity: query_pipeline.proxies[index as usize].entity,
-                    distance: hit.time_of_impact,
-                    point1: hit.witness1.into(),
-                    point2: hit.witness2.into(),
-                    normal1: hit.normal1.into(),
-                    normal2: hit.normal2.into(),
-                });
-
-            if let Some(hit) = hit {
-                query_filter.excluded_entities.insert(hit.entity);
-                hits.push(hit);
-            } else {
-                return;
-            }
-        }
+        let _: (_, Vector) = (shape_isometry, shape_direction);
+        let _ = (caster_entity, query_pipeline);
+        // TODO: Collect hits until we hit max_hits or hit everything
+        // TODO: ignore self
+        // query_pipeline.cast_shape()
     }
 }
 
-fn on_add_shape_caster(mut world: DeferredWorld, ctx: HookContext) {
-    let shape_caster = world.get::<ShapeCaster>(ctx.entity).unwrap();
+fn on_add_shape_caster<C: QueryCollider>(mut world: DeferredWorld, ctx: HookContext) {
+    let shape_caster = world.get::<ShapeCaster<C>>(ctx.entity).unwrap();
     let max_hits = if shape_caster.max_hits == u32::MAX {
         10
     } else {
@@ -522,11 +481,11 @@ impl ShapeCastConfig {
 #[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
 #[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))]
 #[reflect(Component, Debug, Default, PartialEq)]
-pub struct ShapeHits(pub Vec<ShapeHitData>);
+pub struct ShapeHits(pub Vec<ShapeCastHit>);
 
 impl IntoIterator for ShapeHits {
-    type Item = ShapeHitData;
-    type IntoIter = alloc::vec::IntoIter<ShapeHitData>;
+    type Item = ShapeCastHit;
+    type IntoIter = alloc::vec::IntoIter<ShapeCastHit>;
 
     fn into_iter(self) -> Self::IntoIter {
         self.0.into_iter()
@@ -534,8 +493,8 @@ impl IntoIterator for ShapeHits {
 }
 
 impl<'a> IntoIterator for &'a ShapeHits {
-    type Item = &'a ShapeHitData;
-    type IntoIter = core::slice::Iter<'a, ShapeHitData>;
+    type Item = &'a ShapeCastHit;
+    type IntoIter = core::slice::Iter<'a, ShapeCastHit>;
 
     fn into_iter(self) -> Self::IntoIter {
         self.0.iter()
@@ -543,8 +502,8 @@ impl<'a> IntoIterator for &'a ShapeHits {
 }
 
 impl<'a> IntoIterator for &'a mut ShapeHits {
-    type Item = &'a mut ShapeHitData;
-    type IntoIter = core::slice::IterMut<'a, ShapeHitData>;
+    type Item = &'a mut ShapeCastHit;
+    type IntoIter = core::slice::IterMut<'a, ShapeCastHit>;
 
     fn into_iter(self) -> Self::IntoIter {
         self.0.iter_mut()
@@ -564,7 +523,7 @@ impl MapEntities for ShapeHits {
 #[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
 #[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))]
 #[reflect(Debug, PartialEq)]
-pub struct ShapeHitData {
+pub struct ShapeCastHit {
     /// The entity of the collider that was hit by the shape.
     pub entity: Entity,
 
@@ -572,26 +531,45 @@ pub struct ShapeHitData {
     #[doc(alias = "time_of_impact")]
     pub distance: Scalar,
 
-    /// The closest point on the shape that was hit, expressed in world space.
+    /// The hit point in world space.
     ///
-    /// If the shapes are penetrating or the target distance is greater than zero,
-    /// this will be different from `point2`.
-    pub point1: Vector,
+    /// If the shapes are penetrating, this will be the midpoint between the closest points
+    /// on the surfaces of the two shapes.
+    pub point: Vector,
 
-    /// The closest point on the shape that was cast, expressed in world space.
-    ///
-    /// If the shapes are penetrating or the target distance is greater than zero,
-    /// this will be different from `point1`.
-    pub point2: Vector,
+    /// The outward surface normal on the hit shape at the hit point in world space.
+    pub normal: Vector,
+}
 
-    /// The outward surface normal on the hit shape at `point1`, expressed in world space.
-    pub normal1: Vector,
+impl MapEntities for ShapeCastHit {
+    fn map_entities<M: EntityMapper>(&mut self, entity_mapper: &mut M) {
+        self.entity = entity_mapper.get_mapped(self.entity);
+    }
+}
+
+/// Data related to a hit during a [shapecast](spatial_query#shapecasting).
+#[derive(Clone, Copy, Debug, PartialEq, Reflect)]
+#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
+#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))]
+#[reflect(Debug, PartialEq)]
+pub struct ShapeIntersectionHit {
+    /// The entity of the collider that was hit by the shape.
+    pub entity: Entity,
+
+    /// How much the shapes overlap
+    pub penetration: Scalar,
+
+    /// The hit point in world space.
+    ///
+    /// If the shapes are penetrating, this will be the midpoint between the closest points
+    /// on the surfaces of the two shapes.
+    pub point: Vector,
 
-    /// The outward surface normal on the cast shape at `point2`, expressed in world space.
-    pub normal2: Vector,
+    /// The outward surface normal on the hit shape at the hit point in world space.
+    pub normal: Vector,
 }
 
-impl MapEntities for ShapeHitData {
+impl MapEntities for ShapeIntersectionHit {
     fn map_entities<M: EntityMapper>(&mut self, entity_mapper: &mut M) {
         self.entity = entity_mapper.get_mapped(self.entity);
     }
diff --git a/src/spatial_query/system_param.rs b/src/spatial_query/system_param.rs
index 540e07ef..46b4ca66 100644
--- a/src/spatial_query/system_param.rs
+++ b/src/spatial_query/system_param.rs
@@ -1,5 +1,13 @@
-use crate::prelude::*;
-use bevy::{ecs::system::SystemParam, prelude::*};
+use crate::{
+    collision::collider::{BoundedShape, QueryCollider, SingleContext},
+    physics_transform::RotationValue,
+    prelude::*,
+};
+use bevy::{
+    ecs::system::{StaticSystemParam, SystemParam},
+    prelude::*,
+};
+use spatial_query::bvh_ext::RayExt;
 
 /// A system parameter for performing [spatial queries](spatial_query).
 ///
@@ -56,29 +64,21 @@ use bevy::{ecs::system::SystemParam, prelude::*};
 /// }
 /// ```
 #[derive(SystemParam)]
-pub struct SpatialQuery<'w, 's> {
-    pub(crate) colliders: Query<
-        'w,
-        's,
-        (
-            Entity,
-            &'static Position,
-            &'static Rotation,
-            &'static Collider,
-            &'static CollisionLayers,
-        ),
-        Without<ColliderDisabled>,
-    >,
+pub struct SpatialQuery<'w, 's, C: QueryCollider> {
+    /// The query to access colliders
+    pub colliders: Query<'w, 's, (&'static Position, &'static Rotation, &'static C)>,
     /// The [`SpatialQueryPipeline`].
-    pub query_pipeline: ResMut<'w, SpatialQueryPipeline>,
+    pub query_pipeline: Res<'w, SpatialQueryPipeline>,
+    /// The context necessary to use the collider
+    pub context: StaticSystemParam<'w, 's, <C as AnyCollider>::Context>,
 }
 
-impl SpatialQuery<'_, '_> {
+impl<C: QueryCollider> SpatialQuery<'_, '_, C> {
     /// Updates the colliders in the pipeline. This is done automatically once per physics frame in
     /// [`PhysicsStepSystems::SpatialQuery`], but if you modify colliders or their positions before that, you can
     /// call this to make sure the data is up to date when performing spatial queries using [`SpatialQuery`].
     pub fn update_pipeline(&mut self) {
-        self.query_pipeline.update(self.colliders.iter());
+        todo!();
     }
 
     /// Casts a [ray](spatial_query#raycasting) and computes the closest [hit](RayHitData) with a collider.
@@ -132,12 +132,40 @@ impl SpatialQuery<'_, '_> {
         max_distance: Scalar,
         solid: bool,
         filter: &SpatialQueryFilter,
-    ) -> Option<RayHitData> {
+    ) -> Option<RayCastHit> {
         self.query_pipeline
-            .cast_ray(origin, direction, max_distance, solid, filter)
+            .cast_ray(origin, direction, max_distance, filter, |entity, ray| {
+                let Ok((pos, rot, collider)) = self.colliders.get(entity) else {
+                    return f32::INFINITY;
+                };
+                C::ray_hit(
+                    &collider,
+                    ray.transformed(*pos, *rot),
+                    solid,
+                    SingleContext::new(entity, &*self.context),
+                )
+            })
+            .map(|(entity, distance)| {
+                let Ok((pos, rot, collider)) = self.colliders.get(entity) else {
+                    unreachable!(); // Checked previously
+                };
+                let rot_inv = rot.inverse();
+                let normal = C::ray_normal(
+                    &collider,
+                    rot_inv * ((origin + direction * distance) - **pos),
+                    rot_inv * direction,
+                    solid,
+                    SingleContext::new(entity, &*self.context),
+                );
+                RayCastHit {
+                    entity,
+                    distance,
+                    normal: rot * normal,
+                }
+            })
     }
 
-    /// Casts a [ray](spatial_query#raycasting) and computes the closest [hit](RayHitData) with a collider.
+    /// Casts a [ray](spatial_query#raycasting) and computes the closest [hit](RayCastHit) with a collider.
     /// If there are no hits, `None` is returned.
     ///
     /// # Arguments
@@ -199,18 +227,43 @@ impl SpatialQuery<'_, '_> {
         solid: bool,
         filter: &SpatialQueryFilter,
         predicate: &dyn Fn(Entity) -> bool,
-    ) -> Option<RayHitData> {
-        self.query_pipeline.cast_ray_predicate(
-            origin,
-            direction,
-            max_distance,
-            solid,
-            filter,
-            predicate,
-        )
+    ) -> Option<RayCastHit> {
+        self.query_pipeline
+            .cast_ray(origin, direction, max_distance, filter, |entity, ray| {
+                if !predicate(entity) {
+                    return f32::INFINITY;
+                }
+                let Ok((pos, rot, collider)) = self.colliders.get(entity) else {
+                    return f32::INFINITY;
+                };
+                C::ray_hit(
+                    &collider,
+                    ray.transformed(*pos, *rot),
+                    solid,
+                    SingleContext::new(entity, &*self.context),
+                )
+            })
+            .map(|(entity, distance)| {
+                let Ok((pos, rot, collider)) = self.colliders.get(entity) else {
+                    unreachable!(); // Checked previously
+                };
+                let rot_inv = rot.inverse();
+                let normal = C::ray_normal(
+                    &collider,
+                    rot_inv * ((origin + direction * distance) - **pos),
+                    rot_inv * direction,
+                    solid,
+                    SingleContext::new(entity, &*self.context),
+                );
+                RayCastHit {
+                    entity,
+                    distance,
+                    normal: rot * normal,
+                }
+            })
     }
 
-    /// Casts a [ray](spatial_query#raycasting) and computes all [hits](RayHitData) until `max_hits` is reached.
+    /// Casts a [ray](spatial_query#raycasting) and computes all [hits](RayCastHit) until `max_hits` is reached.
     ///
     /// Note that the order of the results is not guaranteed, and if there are more hits than `max_hits`,
     /// some hits will be missed.
@@ -268,12 +321,43 @@ impl SpatialQuery<'_, '_> {
         max_hits: u32,
         solid: bool,
         filter: &SpatialQueryFilter,
-    ) -> Vec<RayHitData> {
+    ) -> Vec<RayCastHit> {
+        let mut hits = Vec::with_capacity(max_hits.min(64) as usize);
+
         self.query_pipeline
-            .ray_hits(origin, direction, max_distance, max_hits, solid, filter)
+            .ray_hits(origin, direction, max_distance, filter, |entity, ray| {
+                let Ok((pos, rot, collider)) = self.colliders.get(entity) else {
+                    return true;
+                };
+                let context = SingleContext::<C::Context>::new(entity, &*self.context);
+                let distance = C::ray_hit(
+                    &collider,
+                    ray.transformed(*pos, *rot),
+                    solid,
+                    context.clone(),
+                );
+                if distance > ray.tmax {
+                    return true;
+                }
+                let rot_inv = rot.inverse();
+                let normal = C::ray_normal(
+                    &collider,
+                    rot_inv * ((origin + direction * distance) - **pos),
+                    rot_inv * direction,
+                    solid,
+                    context,
+                );
+                hits.push(RayCastHit {
+                    entity,
+                    distance,
+                    normal: rot * normal,
+                });
+                hits.len() < max_hits as usize
+            });
+        hits
     }
 
-    /// Casts a [ray](spatial_query#raycasting) and computes all [hits](RayHitData), calling the given `callback`
+    /// Casts a [ray](spatial_query#raycasting) and computes all [hits](RayCastHit), calling the given `callback`
     /// for each hit. The raycast stops when `callback` returns false or all hits have been found.
     ///
     /// Note that the order of the results is not guaranteed.
@@ -334,16 +418,38 @@ impl SpatialQuery<'_, '_> {
         max_distance: Scalar,
         solid: bool,
         filter: &SpatialQueryFilter,
-        callback: impl FnMut(RayHitData) -> bool,
+        callback: &mut dyn FnMut(RayCastHit) -> bool,
     ) {
-        self.query_pipeline.ray_hits_callback(
-            origin,
-            direction,
-            max_distance,
-            solid,
-            filter,
-            callback,
-        )
+        self.query_pipeline
+            .ray_hits(origin, direction, max_distance, filter, |entity, ray| {
+                let Ok((pos, rot, collider)) = self.colliders.get(entity) else {
+                    return true;
+                };
+                let context = SingleContext::<C::Context>::new(entity, &*self.context);
+                let distance = C::ray_hit(
+                    &collider,
+                    ray.transformed(*pos, *rot),
+                    solid,
+                    context.clone(),
+                );
+                if distance > ray.tmax {
+                    return true;
+                }
+                let rot_inv = rot.inverse();
+                let normal = C::ray_normal(
+                    &collider,
+                    rot_inv * ((origin + direction * distance) - **pos),
+                    rot_inv * direction,
+                    solid,
+                    context,
+                );
+                let hit = RayCastHit {
+                    entity,
+                    distance,
+                    normal: rot * normal,
+                };
+                callback(hit)
+            });
     }
 
     /// Casts a [shape](spatial_query#shapecasting) with a given rotation and computes the closest [hit](ShapeHitData)
@@ -397,15 +503,36 @@ impl SpatialQuery<'_, '_> {
     #[allow(clippy::too_many_arguments)]
     pub fn cast_shape(
         &self,
-        shape: &Collider,
+        shape: &C::CastShape, // impl Into<> without mega bloat
         origin: Vector,
         shape_rotation: RotationValue,
         direction: Dir,
         config: &ShapeCastConfig,
         filter: &SpatialQueryFilter,
-    ) -> Option<ShapeHitData> {
+    ) -> Option<ShapeCastHit> {
+        let aabb = shape.shape_aabb(Vector::ZERO, shape_rotation, &*self.context);
+
         self.query_pipeline
-            .cast_shape(shape, origin, shape_rotation, direction, config, filter)
+            .cast_aabb(aabb, origin, direction, config, filter, |entity, ray| {
+                let Ok((pos, rot, collider)) = self.colliders.get(entity) else {
+                    return None;
+                };
+                let rot_inv = rot.inverse();
+                C::shape_cast(
+                    &collider,
+                    shape,
+                    rot_inv * Rotation::from(shape_rotation),
+                    rot_inv * (origin - **pos),
+                    rot_inv * direction,
+                    (ray.tmin, ray.tmax),
+                    SingleContext::new(entity, &*self.context),
+                )
+                .map(|mut hit| {
+                    hit.point = **pos + rot * hit.point;
+                    hit.normal = rot * hit.normal;
+                    hit
+                })
+            })
     }
 
     /// Casts a [shape](spatial_query#shapecasting) with a given rotation and computes the closest [hit](ShapeHitData)
@@ -467,23 +594,40 @@ impl SpatialQuery<'_, '_> {
     /// - [`SpatialQuery::ray_hits_callback`]
     pub fn cast_shape_predicate(
         &self,
-        shape: &Collider,
+        shape: &C::CastShape,
         origin: Vector,
         shape_rotation: RotationValue,
         direction: Dir,
         config: &ShapeCastConfig,
         filter: &SpatialQueryFilter,
         predicate: &dyn Fn(Entity) -> bool,
-    ) -> Option<ShapeHitData> {
-        self.query_pipeline.cast_shape_predicate(
-            shape,
-            origin,
-            shape_rotation,
-            direction,
-            config,
-            filter,
-            predicate,
-        )
+    ) -> Option<ShapeCastHit> {
+        let aabb = shape.shape_aabb(Vector::ZERO, shape_rotation, &*self.context);
+
+        self.query_pipeline
+            .cast_aabb(aabb, origin, direction, config, filter, |entity, ray| {
+                if !predicate(entity) {
+                    return None;
+                }
+                let Ok((pos, rot, collider)) = self.colliders.get(entity) else {
+                    return None;
+                };
+                let rot_inv = rot.inverse();
+                C::shape_cast(
+                    &collider,
+                    shape,
+                    rot_inv * Rotation::from(shape_rotation),
+                    rot_inv * (origin - **pos),
+                    rot_inv * direction,
+                    (ray.tmin, ray.tmax),
+                    SingleContext::new(entity, &*self.context),
+                )
+                .map(|mut hit| {
+                    hit.point = **pos + rot * hit.point;
+                    hit.normal = rot * hit.normal;
+                    hit
+                })
+            })
     }
 
     /// Casts a [shape](spatial_query#shapecasting) with a given rotation and computes computes all [hits](ShapeHitData)
@@ -539,23 +683,47 @@ impl SpatialQuery<'_, '_> {
     #[allow(clippy::too_many_arguments)]
     pub fn shape_hits(
         &self,
-        shape: &Collider,
+        shape: &C::CastShape,
         origin: Vector,
         shape_rotation: RotationValue,
         direction: Dir,
         max_hits: u32,
         config: &ShapeCastConfig,
         filter: &SpatialQueryFilter,
-    ) -> Vec<ShapeHitData> {
-        self.query_pipeline.shape_hits(
-            shape,
-            origin,
-            shape_rotation,
-            direction,
-            max_hits,
-            config,
-            filter,
-        )
+    ) -> Vec<ShapeCastHit> {
+        let mut hits = Vec::with_capacity(max_hits.min(64) as usize);
+        let aabb = shape.shape_aabb(Vector::ZERO, shape_rotation, &*self.context);
+
+        self.query_pipeline
+            .shape_hits(aabb, origin, direction, config, filter, |entity, ray| {
+                let Ok((pos, rot, collider)) = self.colliders.get(entity) else {
+                    return true;
+                };
+
+                let rot_inv = rot.inverse();
+                let hit = C::shape_cast(
+                    &collider,
+                    shape,
+                    rot_inv * Rotation::from(shape_rotation),
+                    rot_inv * (origin - **pos),
+                    rot_inv * direction,
+                    (ray.tmin, ray.tmax),
+                    SingleContext::new(entity, &*self.context),
+                );
+
+                if let Some(hit) = hit {
+                    hits.push(ShapeCastHit {
+                        entity,
+                        distance: hit.distance,
+                        point: **pos + rot * hit.point,
+                        normal: rot * hit.normal,
+                    });
+                }
+
+                true
+            });
+
+        hits
     }
 
     /// Casts a [shape](spatial_query#shapecasting) with a given rotation and computes computes all [hits](ShapeHitData)
@@ -615,23 +783,44 @@ impl SpatialQuery<'_, '_> {
     #[allow(clippy::too_many_arguments)]
     pub fn shape_hits_callback(
         &self,
-        shape: &Collider,
+        shape: &C::CastShape,
         origin: Vector,
         shape_rotation: RotationValue,
         direction: Dir,
         config: &ShapeCastConfig,
         filter: &SpatialQueryFilter,
-        callback: impl FnMut(ShapeHitData) -> bool,
+        callback: &mut dyn FnMut(ShapeCastHit) -> bool,
     ) {
-        self.query_pipeline.shape_hits_callback(
-            shape,
-            origin,
-            shape_rotation,
-            direction,
-            config,
-            filter,
-            callback,
-        )
+        let aabb = shape.shape_aabb(Vector::ZERO, shape_rotation, &*self.context);
+
+        self.query_pipeline
+            .shape_hits(aabb, origin, direction, config, filter, |entity, ray| {
+                let Ok((pos, rot, collider)) = self.colliders.get(entity) else {
+                    return true;
+                };
+
+                let rot_inv = rot.inverse();
+                let hit = C::shape_cast(
+                    &collider,
+                    shape,
+                    rot_inv * Rotation::from(shape_rotation),
+                    rot_inv * (origin - **pos),
+                    rot_inv * direction,
+                    (ray.tmin, ray.tmax),
+                    SingleContext::new(entity, &*self.context),
+                );
+
+                if let Some(hit) = hit {
+                    return callback(ShapeCastHit {
+                        entity,
+                        distance: hit.distance,
+                        point: **pos + rot * hit.point,
+                        normal: rot * hit.normal,
+                    });
+                }
+
+                true
+            });
     }
 
     /// Finds the [projection](spatial_query#point-projection) of a given point on the closest [collider](Collider).
@@ -675,7 +864,21 @@ impl SpatialQuery<'_, '_> {
         solid: bool,
         filter: &SpatialQueryFilter,
     ) -> Option<PointProjection> {
-        self.query_pipeline.project_point(point, solid, filter)
+        self.query_pipeline
+            .project_point(point, filter, |entity, _| {
+                let Ok((pos, rot, collider)) = self.colliders.get(entity) else {
+                    return f32::INFINITY;
+                };
+
+                let hit = C::closest_point(
+                    &collider,
+                    rot.inverse() * (point - **pos),
+                    solid,
+                    SingleContext::new(entity, &*self.context),
+                );
+
+                point.distance(hit)
+            })
     }
 
     /// Finds the [projection](spatial_query#point-projection) of a given point on the closest [collider](Collider).
@@ -729,7 +932,24 @@ impl SpatialQuery<'_, '_> {
         predicate: &dyn Fn(Entity) -> bool,
     ) -> Option<PointProjection> {
         self.query_pipeline
-            .project_point_predicate(point, solid, filter, predicate)
+            .project_point(point, filter, |entity, _| {
+                if !predicate(entity) {
+                    return f32::INFINITY;
+                }
+
+                let Ok((pos, rot, collider)) = self.colliders.get(entity) else {
+                    return f32::INFINITY;
+                };
+
+                let hit = C::closest_point(
+                    &collider,
+                    rot.inverse() * (point - **pos),
+                    solid,
+                    SingleContext::new(entity, &*self.context),
+                );
+
+                point.distance(hit)
+            })
     }
 
     /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [collider](Collider)
@@ -764,7 +984,27 @@ impl SpatialQuery<'_, '_> {
     ///
     /// - [`SpatialQuery::point_intersections_callback`]
     pub fn point_intersections(&self, point: Vector, filter: &SpatialQueryFilter) -> Vec<Entity> {
-        self.query_pipeline.point_intersections(point, filter)
+        let mut hits = Vec::with_capacity(10);
+
+        self.query_pipeline
+            .point_intersections(point, filter, |entity, _| {
+                let Ok((pos, rot, collider)) = self.colliders.get(entity) else {
+                    return (false, true);
+                };
+
+                if C::contains_point(
+                    &collider,
+                    rot.inverse() * (point - **pos),
+                    SingleContext::new(entity, &*self.context),
+                ) {
+                    hits.push(entity);
+                    return (true, true);
+                }
+
+                (false, true)
+            });
+
+        hits
     }
 
     /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [collider](Collider)
@@ -812,10 +1052,24 @@ impl SpatialQuery<'_, '_> {
         &self,
         point: Vector,
         filter: &SpatialQueryFilter,
-        callback: impl FnMut(Entity) -> bool,
+        callback: &mut dyn FnMut(Entity) -> bool,
     ) {
         self.query_pipeline
-            .point_intersections_callback(point, filter, callback)
+            .point_intersections(point, filter, |entity, _| {
+                let Ok((pos, rot, collider)) = self.colliders.get(entity) else {
+                    return (false, true);
+                };
+
+                if C::contains_point(
+                    &collider,
+                    rot.inverse() * (point - **pos),
+                    SingleContext::new(entity, &*self.context),
+                ) {
+                    return (true, callback(entity));
+                }
+
+                (false, true)
+            });
     }
 
     /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [`ColliderAabb`]
@@ -844,8 +1098,19 @@ impl SpatialQuery<'_, '_> {
     /// # Related Methods
     ///
     /// - [`SpatialQuery::aabb_intersections_with_aabb_callback`]
-    pub fn aabb_intersections_with_aabb(&self, aabb: ColliderAabb) -> Vec<Entity> {
-        self.query_pipeline.aabb_intersections_with_aabb(aabb)
+    pub fn aabb_intersections_with_aabb(
+        &self,
+        aabb: ColliderAabb,
+        filter: &SpatialQueryFilter,
+    ) -> Vec<Entity> {
+        let mut hits = Vec::with_capacity(10);
+        self.query_pipeline
+            .aabb_intersections_with_aabb(aabb, filter, |entity| {
+                hits.push(entity);
+                true
+            });
+
+        hits
     }
 
     /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [`ColliderAabb`]
@@ -885,10 +1150,11 @@ impl SpatialQuery<'_, '_> {
     pub fn aabb_intersections_with_aabb_callback(
         &self,
         aabb: ColliderAabb,
-        callback: impl FnMut(Entity) -> bool,
+        filter: &SpatialQueryFilter,
+        callback: &mut dyn FnMut(Entity) -> bool,
     ) {
         self.query_pipeline
-            .aabb_intersections_with_aabb_callback(aabb, callback)
+            .aabb_intersections_with_aabb(aabb, filter, |entity| callback(entity))
     }
 
     /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [`Collider`]
@@ -919,8 +1185,8 @@ impl SpatialQuery<'_, '_> {
     ///         &SpatialQueryFilter::default(),  // Query filter
     ///     );
     ///
-    ///     for entity in intersections.iter() {
-    ///         println!("Entity: {}", entity);
+    ///     for hit in intersections.iter() {
+    ///         println!("Entity: {}", hit.entity);
     ///     }
     /// }
     /// ```
@@ -930,13 +1196,32 @@ impl SpatialQuery<'_, '_> {
     /// - [`SpatialQuery::shape_intersections_callback`]
     pub fn shape_intersections(
         &self,
-        shape: &Collider,
+        shape: &C::Shape,
         shape_position: Vector,
         shape_rotation: RotationValue,
         filter: &SpatialQueryFilter,
     ) -> Vec<Entity> {
+        let aabb = shape.shape_aabb(shape_position, shape_rotation, &*self.context);
+        let mut hits = Vec::with_capacity(10);
         self.query_pipeline
-            .shape_intersections(shape, shape_position, shape_rotation, filter)
+            .aabb_intersections_with_aabb(aabb, filter, |entity| {
+                let Ok((position, rotation, collider)) = self.colliders.get(entity) else {
+                    return true;
+                };
+                let hit = C::shape_intersection(
+                    collider,
+                    shape,
+                    rotation.inverse() * Rotation::from(shape_rotation),
+                    shape_position - **position,
+                    SingleContext::new(entity, &*self.context),
+                );
+                if hit {
+                    hits.push(entity);
+                }
+                true
+            });
+
+        hits
     }
 
     /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [`Collider`]
@@ -969,8 +1254,8 @@ impl SpatialQuery<'_, '_> {
     ///         Vec3::ZERO,                      // Shape position
     ///         Quat::default(),                 // Shape rotation
     ///         &SpatialQueryFilter::default(),  // Query filter
-    ///         |entity| {                       // Callback function
-    ///             intersections.push(entity);
+    ///         |hit| {                       // Callback function
+    ///             intersections.push(hit.entity);
     ///             true
     ///         },
     ///     );
@@ -986,18 +1271,29 @@ impl SpatialQuery<'_, '_> {
     /// - [`SpatialQuery::shape_intersections`]
     pub fn shape_intersections_callback(
         &self,
-        shape: &Collider,
+        shape: &C::Shape,
         shape_position: Vector,
         shape_rotation: RotationValue,
         filter: &SpatialQueryFilter,
-        callback: impl FnMut(Entity) -> bool,
+        callback: &mut dyn FnMut(Entity) -> bool,
     ) {
-        self.query_pipeline.shape_intersections_callback(
-            shape,
-            shape_position,
-            shape_rotation,
-            filter,
-            callback,
-        )
+        let aabb = shape.shape_aabb(shape_position, shape_rotation, &*self.context);
+        self.query_pipeline
+            .aabb_intersections_with_aabb(aabb, filter, |entity| {
+                let Ok((position, rotation, collider)) = self.colliders.get(entity) else {
+                    return true;
+                };
+                let hit = C::shape_intersection(
+                    collider,
+                    shape,
+                    rotation.inverse() * Rotation::from(shape_rotation),
+                    shape_position - **position,
+                    SingleContext::new(entity, &*self.context),
+                );
+                if hit {
+                    return callback(entity);
+                }
+                true
+            });
     }
 }