[add] physics point and aabb queries

Author: vmarcella

crates/lambda-rs-platform/src/physics/rapier2d.rs

@@ -150,6 +150,10 @@ struct RigidBodySlot2D {
 struct ColliderSlot2D {
   /// The handle to the Rapier collider stored in the `ColliderSet`.
   rapier_handle: ColliderHandle,
+  /// The parent rigid body slot index that owns this collider.
+  parent_slot_index: u32,
+  /// The parent rigid body slot generation that owns this collider.
+  parent_slot_generation: u32,
   /// A monotonically increasing counter used to validate stale handles.
   generation: u32,
 }
@@ -620,6 +624,85 @@ impl PhysicsBackend2D {
       .is_ok();
   }
 
+  /// Returns all rigid bodies whose colliders contain the provided point.
+  ///
+  /// # Arguments
+  /// - `point`: The world-space point to test.
+  ///
+  /// # Returns
+  /// Returns backend rigid body slot pairs for each matching collider.
+  pub fn query_point_2d(&self, point: [f32; 2]) -> Vec<(u32, u32)> {
+    if validate_position(point[0], point[1]).is_err() {
+      return Vec::new();
+    }
+
+    let point = Vector::new(point[0], point[1]);
+    let mut body_slots = Vec::new();
+
+    for (collider_handle, collider) in self.colliders.iter() {
+      if !collider.shape().contains_point(collider.position(), point) {
+        continue;
+      }
+
+      let Some(body_slot) =
+        self.query_hit_to_parent_body_slot_2d(collider_handle)
+      else {
+        continue;
+      };
+
+      body_slots.push(body_slot);
+    }
+
+    return body_slots;
+  }
+
+  /// Returns all rigid bodies whose colliders overlap the provided AABB.
+  ///
+  /// # Arguments
+  /// - `min`: The minimum world-space corner of the query box.
+  /// - `max`: The maximum world-space corner of the query box.
+  ///
+  /// # Returns
+  /// Returns backend rigid body slot pairs for each matching collider.
+  pub fn query_aabb_2d(&self, min: [f32; 2], max: [f32; 2]) -> Vec<(u32, u32)> {
+    if validate_position(min[0], min[1]).is_err()
+      || validate_position(max[0], max[1]).is_err()
+    {
+      return Vec::new();
+    }
+
+    let half_extents =
+      Vector::new((max[0] - min[0]) * 0.5, (max[1] - min[1]) * 0.5);
+    let center = Vector::new((min[0] + max[0]) * 0.5, (min[1] + max[1]) * 0.5);
+    let query_shape = Cuboid::new(half_extents);
+    let query_pose = Pose::from_translation(center);
+    let query_dispatcher = self.narrow_phase.query_dispatcher();
+    let mut body_slots = Vec::new();
+
+    for (collider_handle, collider) in self.colliders.iter() {
+      let shape_to_collider = query_pose.inv_mul(collider.position());
+      let intersects = query_dispatcher.intersection_test(
+        &shape_to_collider,
+        &query_shape,
+        collider.shape(),
+      );
+
+      if intersects != Ok(true) {
+        continue;
+      }
+
+      let Some(body_slot) =
+        self.query_hit_to_parent_body_slot_2d(collider_handle)
+      else {
+        continue;
+      };
+
+      body_slots.push(body_slot);
+    }
+
+    return body_slots;
+  }
+
   /// Sets the current position for the referenced body.
   ///
   /// # Arguments
@@ -1101,6 +1184,8 @@ impl PhysicsBackend2D {
     let slot_generation = 1;
     self.collider_slots_2d.push(ColliderSlot2D {
       rapier_handle,
+      parent_slot_index,
+      parent_slot_generation,
       generation: slot_generation,
     });
 
@@ -1222,10 +1307,41 @@ impl PhysicsBackend2D {
         self.colliders.get(slot.rapier_handle).is_some(),
         "collider slot references missing Rapier collider"
       );
+      debug_assert!(
+        self
+          .rigid_body_slot_2d(
+            slot.parent_slot_index,
+            slot.parent_slot_generation
+          )
+          .is_ok(),
+        "collider slot references missing parent rigid body slot"
+      );
     }
 
     return;
   }
+  /// Resolves a query hit collider back to its owning rigid body slot.
+  ///
+  /// # Arguments
+  /// - `collider_handle`: The Rapier collider handle returned by a query.
+  ///
+  /// # Returns
+  /// Returns the owning backend rigid body slot pair when the collider slot is
+  /// still tracked by the backend.
+  fn query_hit_to_parent_body_slot_2d(
+    &self,
+    collider_handle: ColliderHandle,
+  ) -> Option<(u32, u32)> {
+    let collider_slot = self
+      .collider_slots_2d
+      .iter()
+      .find(|slot| slot.rapier_handle == collider_handle)?;
+
+    return Some((
+      collider_slot.parent_slot_index,
+      collider_slot.parent_slot_generation,
+    ));
+  }
 }
 
 /// Builds a Rapier rigid body builder with `lambda-rs` invariants applied.

crates/lambda-rs/src/physics/mod.rs

@@ -4,6 +4,7 @@
 //! API is backend-agnostic and does not expose vendor types.
 
 use std::{
+  collections::HashSet,
   error::Error,
   fmt,
   sync::atomic::{
@@ -157,8 +158,13 @@ impl PhysicsWorld2D {
   ///
   /// # Returns
   /// Returns a vector of matching rigid body handles.
-  pub fn query_point(&self, _point: [f32; 2]) -> Vec<RigidBody2D> {
-    return Vec::new();
+  pub fn query_point(&self, point: [f32; 2]) -> Vec<RigidBody2D> {
+    if !is_valid_query_point(point) {
+      return Vec::new();
+    }
+
+    let body_slots = self.backend.query_point_2d(point);
+    return self.deduplicate_query_body_hits(body_slots);
   }
 
   /// Returns all bodies whose colliders overlap the provided axis-aligned box.
@@ -169,8 +175,16 @@ impl PhysicsWorld2D {
   ///
   /// # Returns
   /// Returns a vector of matching rigid body handles.
-  pub fn query_aabb(&self, _min: [f32; 2], _max: [f32; 2]) -> Vec<RigidBody2D> {
-    return Vec::new();
+  pub fn query_aabb(&self, min: [f32; 2], max: [f32; 2]) -> Vec<RigidBody2D> {
+    if !is_valid_query_point(min) || !is_valid_query_point(max) {
+      return Vec::new();
+    }
+
+    let normalized_min = [min[0].min(max[0]), min[1].min(max[1])];
+    let normalized_max = [min[0].max(max[0]), min[1].max(max[1])];
+    let body_slots = self.backend.query_aabb_2d(normalized_min, normalized_max);
+
+    return self.deduplicate_query_body_hits(body_slots);
   }
 
   /// Returns the nearest rigid body hit by the provided ray.
@@ -190,6 +204,36 @@ impl PhysicsWorld2D {
   ) -> Option<RaycastHit> {
     return None;
   }
+
+  /// Rebuilds and deduplicates rigid body handles from backend query hits.
+  ///
+  /// # Arguments
+  /// - `body_slots`: Backend `(slot_index, slot_generation)` pairs.
+  ///
+  /// # Returns
+  /// Returns one `RigidBody2D` handle per unique body, preserving first-hit
+  /// order from the backend query.
+  fn deduplicate_query_body_hits(
+    &self,
+    body_slots: Vec<(u32, u32)>,
+  ) -> Vec<RigidBody2D> {
+    let mut seen_bodies = HashSet::new();
+    let mut bodies = Vec::new();
+
+    for (slot_index, slot_generation) in body_slots {
+      let body = RigidBody2D::from_backend_slot(
+        self.world_id,
+        slot_index,
+        slot_generation,
+      );
+
+      if seen_bodies.insert(body) {
+        bodies.push(body);
+      }
+    }
+
+    return bodies;
+  }
 }
 
 /// Builder for `PhysicsWorld2D`.
@@ -354,6 +398,17 @@ fn validate_gravity(gravity: [f32; 2]) -> Result<(), PhysicsWorld2DError> {
   return Ok(());
 }
 
+/// Returns whether a query point contains only finite coordinates.
+///
+/// # Arguments
+/// - `point`: The point to validate.
+///
+/// # Returns
+/// Returns `true` when both coordinates are finite.
+fn is_valid_query_point(point: [f32; 2]) -> bool {
+  return point[0].is_finite() && point[1].is_finite();
+}
+
 /// Allocates a non-zero unique world identifier.
 fn allocate_world_id() -> u64 {
   loop {

crates/lambda-rs/src/physics/rigid_body_2d.rs

@@ -44,6 +44,30 @@ pub struct RigidBody2D {
 }
 
 impl RigidBody2D {
+  /// Creates a world-scoped rigid body handle from backend slot identifiers.
+  ///
+  /// This helper is an internal implementation detail used by query and event
+  /// APIs that reconstruct public handles from backend state.
+  ///
+  /// # Arguments
+  /// - `world_id`: The owning physics world identifier.
+  /// - `slot_index`: The backend body slot index.
+  /// - `slot_generation`: The backend body slot generation counter.
+  ///
+  /// # Returns
+  /// Returns a `RigidBody2D` handle bound to the provided world.
+  pub(super) fn from_backend_slot(
+    world_id: u64,
+    slot_index: u32,
+    slot_generation: u32,
+  ) -> Self {
+    return Self {
+      world_id,
+      slot_index,
+      slot_generation,
+    };
+  }
+
   /// Returns the backend slot identifiers for this handle.
   ///
   /// This function is an internal implementation detail used by other

crates/lambda-rs/tests/physics_2d/mod.rs

@@ -7,6 +7,7 @@ mod colliders;
 mod collision_filters;
 mod compound_colliders;
 mod materials;
+mod queries;
 
 use lambda::physics::{
   PhysicsWorld2DBuilder,