Start implementing ray-casting.

This adds a QueryPipeline structure responsible for scene queries.
Currently this structure is able to perform a brute-force ray-cast.
This commit also includes the beginning of implementation of a SIMD-based acceleration structure which will be used for these scene queries in the future.

CHANGELOG

@@ -5,6 +5,7 @@
 - Add `ColliderDesc::rotation(...)` to set the rotation of a collider relative to the rigid-body it is attached to.
 - Add `ColliderDesc::position(...)` to set the position of a collider relative to the rigid-body it is attached to.
 - Add `Collider::position_wrt_parent()` to get the position of a collider relative to the rigid-body it is attached to.
+- Modify `RigidBody::set_position(...)` so it also resets the next kinematic position to the same value.
 - Deprecate `Collider::delta()` in favor of the new `Collider::position_wrt_parent()`.
 - Fix multiple issues occurring when having colliders resulting in a non-zero center-of-mass.
 - Fix a crash happening when removing a rigid-body with a collider, stepping the simulation, adding another rigid-body

src/dynamics/rigid_body.rs

@@ -137,6 +137,15 @@ impl RigidBody {
         crate::utils::inv(self.mass_properties.inv_mass)
     }
 
+    /// The predicted position of this rigid-body.
+    ///
+    /// If this rigid-body is kinematic this value is set by the `set_next_kinematic_position`
+    /// method and is used for estimating the kinematic body velocity at the next timestep.
+    /// For non-kinematic bodies, this value is currently unspecified.
+    pub fn predicted_position(&self) -> &Isometry<f32> {
+        &self.predicted_position
+    }
+
     /// Adds a collider to this rigid-body.
     pub(crate) fn add_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
         let mass_properties = coll
@@ -201,12 +210,17 @@ impl RigidBody {
         self.position = self.integrate_velocity(dt) * self.position;
     }
 
-    /// Sets the position of this rigid body.
+    /// Sets the position and `next_kinematic_position` of this rigid body.
+    ///
+    /// This will teleport the rigid-body to the specified position/orientation,
+    /// completely ignoring any physics rule. If this body is kinematic, this will
+    /// also set the next kinematic position to the same value, effectively
+    /// resetting to zero the next interpolated velocity of the kinematic body.
     pub fn set_position(&mut self, pos: Isometry<f32>) {
         self.position = pos;
 
         // TODO: update the predicted position for dynamic bodies too?
-        if self.is_static() {
+        if self.is_static() || self.is_kinematic() {
             self.predicted_position = pos;
         }
     }

src/geometry/collider.rs

@@ -1,11 +1,12 @@
 use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
 use crate::geometry::{
     Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
-    Proximity, Triangle, Trimesh,
+    Proximity, Ray, RayIntersection, Triangle, Trimesh,
 };
 use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
 use na::Point3;
 use ncollide::bounding_volume::{HasBoundingVolume, AABB};
+use ncollide::query::RayCast;
 use num::Zero;
 
 #[derive(Clone)]
@@ -97,6 +98,46 @@ impl Shape {
             Shape::HeightField(heightfield) => heightfield.bounding_volume(position),
         }
     }
+
+    /// Computes the first intersection point between a ray in this collider.
+    ///
+    /// Some shapes are not supported yet and will always return `None`.
+    ///
+    /// # Parameters
+    /// - `position`: the position of this shape.
+    /// - `ray`: the ray to cast.
+    /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively
+    ///   limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray.
+    pub fn cast_ray(
+        &self,
+        position: &Isometry<f32>,
+        ray: &Ray,
+        max_toi: f32,
+    ) -> Option<RayIntersection> {
+        match self {
+            Shape::Ball(ball) => ball.toi_and_normal_with_ray(position, ray, max_toi, true),
+            Shape::Polygon(_poly) => None,
+            Shape::Capsule(caps) => {
+                let pos = position * caps.transform_wrt_y();
+                let caps = ncollide::shape::Capsule::new(caps.half_height(), caps.radius);
+                caps.toi_and_normal_with_ray(&pos, ray, max_toi, true)
+            }
+            Shape::Cuboid(cuboid) => cuboid.toi_and_normal_with_ray(position, ray, max_toi, true),
+            #[cfg(feature = "dim2")]
+            Shape::Triangle(triangle) => {
+                // This is not implemented yet in 2D.
+                None
+            }
+            #[cfg(feature = "dim3")]
+            Shape::Triangle(triangle) => {
+                triangle.toi_and_normal_with_ray(position, ray, max_toi, true)
+            }
+            Shape::Trimesh(_trimesh) => None,
+            Shape::HeightField(heightfield) => {
+                heightfield.toi_and_normal_with_ray(position, ray, max_toi, true)
+            }
+        }
+    }
 }
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -353,6 +394,12 @@ impl ColliderBuilder {
         self
     }
 
+    /// Sets the restitution coefficient of the collider this builder will build.
+    pub fn restitution(mut self, restitution: f32) -> Self {
+        self.restitution = restitution;
+        self
+    }
+
     /// Sets the density of the collider this builder will build.
     pub fn density(mut self, density: f32) -> Self {
         self.density = Some(density);

src/geometry/mod.rs

@@ -36,6 +36,10 @@ pub type AABB = ncollide::bounding_volume::AABB<f32>;
 pub type ContactEvent = ncollide::pipeline::ContactEvent<ColliderHandle>;
 /// Event triggered when a sensor collider starts or stop being in proximity with another collider (sensor or not).
 pub type ProximityEvent = ncollide::pipeline::ProximityEvent<ColliderHandle>;
+/// A ray that can be cast against colliders.
+pub type Ray = ncollide::query::Ray<f32>;
+/// The intersection between a ray and a  collider.
+pub type RayIntersection = ncollide::query::RayIntersection<f32>;
 
 #[cfg(feature = "simd-is-enabled")]
 pub(crate) use self::ball::WBall;
@@ -48,7 +52,6 @@ pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_norma
 pub(crate) use self::narrow_phase::ContactManifoldIndex;
 #[cfg(feature = "dim3")]
 pub(crate) use self::polyhedron_feature3d::PolyhedronFace;
-#[cfg(feature = "simd-is-enabled")]
 pub(crate) use self::waabb::WAABB;
 //pub(crate) use self::z_order::z_cmp_floats;
 
@@ -75,6 +78,5 @@ mod proximity_detector;
 pub(crate) mod sat;
 pub(crate) mod triangle;
 mod trimesh;
-#[cfg(feature = "simd-is-enabled")]
 mod waabb;
 //mod z_order;

src/geometry/waabb.rs

@@ -1,15 +1,27 @@
 #[cfg(feature = "serde-serialize")]
 use crate::math::DIM;
-use crate::math::{Point, SimdBool, SimdFloat, SIMD_WIDTH};
+use crate::math::{Point, SIMD_WIDTH};
 use ncollide::bounding_volume::AABB;
-use simba::simd::{SimdPartialOrd, SimdValue};
+#[cfg(feature = "simd-is-enabled")]
+use {
+    crate::math::{SimdBool, SimdFloat},
+    simba::simd::{SimdPartialOrd, SimdValue},
+};
 
 #[derive(Debug, Copy, Clone)]
+#[cfg(feature = "simd-is-enabled")]
 pub(crate) struct WAABB {
     pub mins: Point<SimdFloat>,
     pub maxs: Point<SimdFloat>,
 }
 
+#[derive(Debug, Copy, Clone)]
+#[cfg(not(feature = "simd-is-enabled"))]
+pub(crate) struct WAABB {
+    pub mins: [Point<f32>; SIMD_WIDTH],
+    pub maxs: [Point<f32>; SIMD_WIDTH],
+}
+
 #[cfg(feature = "serde-serialize")]
 impl serde::Serialize for WAABB {
     fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
@@ -18,16 +30,24 @@ impl serde::Serialize for WAABB {
     {
         use serde::ser::SerializeStruct;
 
+        #[cfg(feature = "simd-is-enabled")]
         let mins: Point<[f32; SIMD_WIDTH]> = Point::from(
             self.mins
                 .coords
                 .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]),
         );
+        #[cfg(feature = "simd-is-enabled")]
         let maxs: Point<[f32; SIMD_WIDTH]> = Point::from(
             self.maxs
                 .coords
                 .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]),
         );
+
+        #[cfg(not(feature = "simd-is-enabled"))]
+        let mins = self.mins;
+        #[cfg(not(feature = "simd-is-enabled"))]
+        let maxs = self.maxs;
+
         let mut waabb = serializer.serialize_struct("WAABB", 2)?;
         waabb.serialize_field("mins", &mins)?;
         waabb.serialize_field("maxs", &maxs)?;
@@ -52,6 +72,7 @@ impl<'de> serde::Deserialize<'de> for WAABB {
                 )
             }
 
+            #[cfg(feature = "simd-is-enabled")]
             fn visit_seq<A>(self, mut seq: A) -> Result<Self::Value, A::Error>
             where
                 A: serde::de::SeqAccess<'de>,
@@ -66,17 +87,36 @@ impl<'de> serde::Deserialize<'de> for WAABB {
                 let maxs = Point::from(maxs.coords.map(|e| SimdFloat::from(e)));
                 Ok(WAABB { mins, maxs })
             }
+
+            #[cfg(not(feature = "simd-is-enabled"))]
+            fn visit_seq<A>(self, mut seq: A) -> Result<Self::Value, A::Error>
+            where
+                A: serde::de::SeqAccess<'de>,
+            {
+                let mins = seq
+                    .next_element()?
+                    .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
+                let maxs = seq
+                    .next_element()?
+                    .ok_or_else(|| serde::de::Error::invalid_length(1, &self))?;
+                Ok(WAABB { mins, maxs })
+            }
         }
 
         deserializer.deserialize_struct("WAABB", &["mins", "maxs"], Visitor {})
     }
 }
 
+#[cfg(feature = "simd-is-enabled")]
 impl WAABB {
     pub fn new(mins: Point<SimdFloat>, maxs: Point<SimdFloat>) -> Self {
         Self { mins, maxs }
     }
 
+    pub fn new_invalid() -> Self {
+        Self::splat(AABB::new_invalid())
+    }
+
     pub fn splat(aabb: AABB<f32>) -> Self {
         Self {
             mins: Point::splat(aabb.mins),
@@ -103,6 +143,7 @@ impl WAABB {
     }
 }
 
+#[cfg(feature = "simd-is-enabled")]
 impl From<[AABB<f32>; SIMD_WIDTH]> for WAABB {
     fn from(aabbs: [AABB<f32>; SIMD_WIDTH]) -> Self {
         let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH];
@@ -114,3 +155,51 @@ impl From<[AABB<f32>; SIMD_WIDTH]> for WAABB {
         }
     }
 }
+
+#[cfg(not(feature = "simd-is-enabled"))]
+impl WAABB {
+    pub fn new_invalid() -> Self {
+        Self::splat(AABB::new_invalid())
+    }
+
+    pub fn splat(aabb: AABB<f32>) -> Self {
+        Self {
+            mins: [aabb.mins; SIMD_WIDTH],
+            maxs: [aabb.maxs; SIMD_WIDTH],
+        }
+    }
+
+    #[cfg(feature = "dim2")]
+    pub fn intersects_lanewise(&self, other: &WAABB) -> [bool; SIMD_WIDTH] {
+        array![|ii|
+            self.mins[ii].x <= other.maxs[ii].x
+                && other.mins[ii].x <= self.maxs[ii].x
+                && self.mins[ii].y <= other.maxs[ii].y
+                && other.mins[ii].y <= self.maxs[ii].y
+            ; SIMD_WIDTH
+        ]
+    }
+
+    #[cfg(feature = "dim3")]
+    pub fn intersects_lanewise(&self, other: &WAABB) -> [bool; SIMD_WIDTH] {
+        array![|ii|
+            self.mins[ii].x <= other.maxs[ii].x
+                && other.mins[ii].x <= self.maxs[ii].x
+                && self.mins[ii].y <= other.maxs[ii].y
+                && other.mins[ii].y <= self.maxs[ii].y
+                && self.mins[ii].z <= other.maxs[ii].z
+                && other.mins[ii].z <= self.maxs[ii].z
+            ; SIMD_WIDTH
+        ]
+    }
+}
+
+#[cfg(not(feature = "simd-is-enabled"))]
+impl From<[AABB<f32>; SIMD_WIDTH]> for WAABB {
+    fn from(aabbs: [AABB<f32>; SIMD_WIDTH]) -> Self {
+        let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH];
+        let maxs = array![|ii| aabbs[ii].maxs; SIMD_WIDTH];
+
+        WAABB { mins, maxs }
+    }
+}

src/lib.rs

@@ -44,7 +44,6 @@ macro_rules! enable_flush_to_zero(
     }
 );
 
-#[cfg(feature = "simd-is-enabled")]
 macro_rules! array(
     ($callback: expr; SIMD_WIDTH) => {
         {
@@ -139,7 +138,6 @@ pub mod utils;
 #[cfg(feature = "dim2")]
 /// Math primitives used throughout Rapier.
 pub mod math {
-    #[cfg(feature = "simd-is-enabled")]
     pub use super::simd::*;
     use na::{Isometry2, Matrix2, Point2, Translation2, UnitComplex, Vector2, Vector3, U1, U2};
 
@@ -182,7 +180,6 @@ pub mod math {
 #[cfg(feature = "dim3")]
 /// Math primitives used throughout Rapier.
 pub mod math {
-    #[cfg(feature = "simd-is-enabled")]
     pub use super::simd::*;
     use na::{Isometry3, Matrix3, Point3, Translation3, UnitQuaternion, Vector3, Vector6, U3};
 
@@ -220,6 +217,12 @@ pub mod math {
     pub type SdpMatrix<N> = crate::utils::SdpMatrix3<N>;
 }
 
+#[cfg(not(feature = "simd-is-enabled"))]
+mod simd {
+    /// The number of lanes of a SIMD number.
+    pub const SIMD_WIDTH: usize = 4;
+}
+
 #[cfg(feature = "simd-is-enabled")]
 mod simd {
     #[allow(unused_imports)]

src/pipeline/mod.rs

@@ -3,7 +3,9 @@
 pub use collision_pipeline::CollisionPipeline;
 pub use event_handler::{ChannelEventCollector, EventHandler};
 pub use physics_pipeline::PhysicsPipeline;
+pub use query_pipeline::QueryPipeline;
 
 mod collision_pipeline;
 mod event_handler;
 mod physics_pipeline;
+mod query_pipeline;

src/pipeline/query_pipeline.rs

@@ -0,0 +1,306 @@
+use crate::dynamics::RigidBodySet;
+use crate::geometry::{Collider, ColliderHandle, ColliderSet, Ray, RayIntersection, AABB, WAABB};
+use crate::math::{Point, Vector};
+use ncollide::bounding_volume::BoundingVolume;
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+struct NodeIndex {
+    index: u32, // Index of the addressed node in the `children` array.
+    lane: u8,   // SIMD lane of the addressed node.
+}
+
+impl NodeIndex {
+    fn new(index: u32, lane: u8) -> Self {
+        Self { index, lane }
+    }
+
+    fn invalid() -> Self {
+        Self {
+            index: u32::MAX,
+            lane: 0,
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+struct WAABBHierarchyNodeChildren {
+    waabb: WAABB,
+    // Index of the children of the 4 nodes represented by self.
+    // If this is a leaf, it contains the proxy ids instead.
+    grand_children: [u32; 4],
+    parent: NodeIndex,
+    leaf: bool, // TODO: pack this with the NodexIndex.lane?
+}
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+struct ColliderNodeIndex {
+    node: NodeIndex,
+    handle: ColliderHandle, // The collider handle. TODO: only set the collider generation here?
+}
+
+impl ColliderNodeIndex {
+    fn invalid() -> Self {
+        Self {
+            node: NodeIndex::invalid(),
+            handle: ColliderSet::invalid_handle(),
+        }
+    }
+}
+
+struct WAABBHierarchy {
+    children: Vec<WAABBHierarchyNodeChildren>,
+    dirty: Vec<bool>, // TODO: use a bitvec/Vob and check it does not break cross-platform determinism.
+    proxies: Vec<ColliderNodeIndex>,
+}
+
+impl WAABBHierarchy {
+    pub fn new() -> Self {
+        WAABBHierarchy {
+            children: Vec::new(),
+            dirty: Vec::new(),
+            proxies: Vec::new(),
+        }
+    }
+
+    pub fn clear_and_rebuild(&mut self, colliders: &ColliderSet) {
+        self.children.clear();
+        self.dirty.clear();
+        self.proxies.clear();
+
+        // Create proxies.
+        let mut indices = Vec::with_capacity(colliders.len());
+        let mut proxies = vec![ColliderNodeIndex::invalid(); colliders.len()];
+        for (handle, collider) in colliders.iter() {
+            let index = handle.into_raw_parts().0;
+            if proxies.len() < handle.into_raw_parts().0 {
+                proxies.resize(index + 1, ColliderNodeIndex::invalid());
+            }
+
+            proxies[index].handle = handle;
+            indices.push(index);
+        }
+
+        // Compute AABBs.
+        let mut aabbs = vec![AABB::new_invalid(); proxies.len()];
+        for (handle, collider) in colliders.iter() {
+            let index = handle.into_raw_parts().0;
+            let aabb = collider.compute_aabb();
+            aabbs[index] = aabb;
+        }
+
+        // Build the tree recursively.
+        let root_node = WAABBHierarchyNodeChildren {
+            waabb: WAABB::new_invalid(),
+            grand_children: [1; 4],
+            parent: NodeIndex::invalid(),
+            leaf: false,
+        };
+
+        self.children.push(root_node);
+        let root_id = NodeIndex::new(0, 0);
+        let (_, aabb) = self.do_recurse_build(&mut indices, &aabbs, root_id);
+        self.children[0].waabb = WAABB::splat(aabb);
+    }
+
+    fn do_recurse_build(
+        &mut self,
+        indices: &mut [usize],
+        aabbs: &[AABB],
+        parent: NodeIndex,
+    ) -> (u32, AABB) {
+        // Leaf case.
+        if indices.len() <= 4 {
+            let my_id = self.children.len();
+            let mut my_aabb = AABB::new_invalid();
+            let mut leaf_aabbs = [AABB::new_invalid(); 4];
+            let mut proxy_ids = [u32::MAX; 4];
+
+            for (k, id) in indices.iter().enumerate() {
+                my_aabb.merge(&aabbs[*id]);
+                leaf_aabbs[k] = aabbs[*id];
+                proxy_ids[k] = *id as u32;
+            }
+
+            let node = WAABBHierarchyNodeChildren {
+                waabb: WAABB::from(leaf_aabbs),
+                grand_children: proxy_ids,
+                parent,
+                leaf: true,
+            };
+
+            self.children.push(node);
+            return (my_id as u32, my_aabb);
+        }
+
+        // Compute the center and variance along each dimension.
+        // In 3D we compute the variance to not-subdivide the dimension with lowest variance.
+        // Therefore variance computation is not needed in 2D because we only have 2 dimension
+        // to split in the first place.
+        let mut center = Point::origin();
+        #[cfg(feature = "dim3")]
+        let mut variance = Vector::zeros();
+
+        let denom = 1.0 / (indices.len() as f32);
+
+        for i in &*indices {
+            let coords = aabbs[*i].center().coords;
+            center += coords * denom;
+            #[cfg(feature = "dim3")]
+            {
+                variance += coords.component_mul(&coords) * denom;
+            }
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            variance = variance - center.coords.component_mul(&center.coords);
+        }
+
+        // Find the axis with minimum variance. This is the axis along
+        // which we are **not** subdividing our set.
+        let mut subdiv_dims = [0, 1];
+        #[cfg(feature = "dim3")]
+        {
+            let min = variance.imin();
+            subdiv_dims[0] = (min + 1) % 3;
+            subdiv_dims[1] = (min + 2) % 3;
+        }
+
+        // Split the set along the two subdiv_dims dimensions.
+        // TODO: should we split wrt. the median instead of the average?
+        // TODO: we should ensure each subslice contains at least 4 elements each (or less if
+        // indices has less than 16 elements in the first place.
+        let (left, right) = split_indices_wrt_dim(indices, &aabbs, subdiv_dims[0]);
+        let (left_bottom, left_top) = split_indices_wrt_dim(left, &aabbs, subdiv_dims[1]);
+        let (right_bottom, right_top) = split_indices_wrt_dim(right, &aabbs, subdiv_dims[1]);
+
+        let node = WAABBHierarchyNodeChildren {
+            waabb: WAABB::new_invalid(),
+            grand_children: [0; 4], // Will be set after the recursive call
+            parent,
+            leaf: false,
+        };
+
+        let id = self.children.len() as u32;
+        self.children.push(node);
+
+        // Recurse!
+        let a = self.do_recurse_build(left_bottom, aabbs, NodeIndex::new(id, 0));
+        let b = self.do_recurse_build(left_top, aabbs, NodeIndex::new(id, 1));
+        let c = self.do_recurse_build(right_bottom, aabbs, NodeIndex::new(id, 2));
+        let d = self.do_recurse_build(right_top, aabbs, NodeIndex::new(id, 3));
+
+        // Now we know the indices of the grand-children.
+        self.children[id as usize].grand_children = [a.0, b.0, c.0, d.0];
+        self.children[id as usize].waabb = WAABB::from([a.1, b.1, c.1, d.1]);
+
+        // TODO: will this chain of .merged be properly optimized?
+        let my_aabb = a.1.merged(&b.1).merged(&c.1).merged(&c.1);
+        (id, my_aabb)
+    }
+}
+
+fn split_indices_wrt_dim<'a>(
+    indices: &'a mut [usize],
+    aabbs: &[AABB],
+    dim: usize,
+) -> (&'a mut [usize], &'a mut [usize]) {
+    let mut icurr = 0;
+    let mut ilast = indices.len() - 1;
+
+    // The loop condition we can just do 0..indices.len()
+    // instead of the test icurr < ilast because we know
+    // we will iterate exactly once per index.
+    for _ in 0..indices.len() {
+        let i = indices[icurr];
+        let center = aabbs[i].center();
+
+        if center[dim] > center[dim] {
+            indices.swap(icurr, ilast);
+            ilast -= 1;
+        } else {
+            icurr += 1;
+        }
+    }
+
+    indices.split_at_mut(icurr)
+}
+
+/// A pipeline for performing queries on all the colliders of a scene.
+pub struct QueryPipeline {
+    // hierarchy: WAABBHierarchy,
+}
+
+impl Default for QueryPipeline {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+impl QueryPipeline {
+    /// Initializes an empty query pipeline.
+    pub fn new() -> Self {
+        Self {
+            // hierarchy: WAABBHierarchy::new(),
+        }
+    }
+
+    /// Update the acceleration structure on the query pipeline.
+    pub fn update(&mut self, _bodies: &mut RigidBodySet, _colliders: &mut ColliderSet) {}
+
+    /// Find the closest intersection between a ray and a set of collider.
+    ///
+    /// # Parameters
+    /// - `position`: the position of this shape.
+    /// - `ray`: the ray to cast.
+    /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively
+    ///   limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray.
+    pub fn cast_ray<'a>(
+        &self,
+        colliders: &'a ColliderSet,
+        ray: &Ray,
+        max_toi: f32,
+    ) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> {
+        let mut best = f32::MAX;
+        let mut result = None;
+
+        // FIXME: this is a brute-force approach.
+        for (handle, collider) in colliders.iter() {
+            if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) {
+                if inter.toi < best {
+                    best = inter.toi;
+                    result = Some((handle, collider, inter));
+                }
+            }
+        }
+
+        result
+    }
+
+    /// Find the all intersections between a ray and a set of collider and passes them to a callback.
+    ///
+    /// # Parameters
+    /// - `position`: the position of this shape.
+    /// - `ray`: the ray to cast.
+    /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively
+    ///   limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray.
+    /// - `callback`: function executed on each collider for which a ray intersection has been found.
+    ///   There is no guarantees on the order the results will be yielded. If this callback returns `false`,
+    ///   this method will exit early, ignory any further raycast.
+    pub fn interferences_with_ray<'a>(
+        &self,
+        colliders: &'a ColliderSet,
+        ray: &Ray,
+        max_toi: f32,
+        mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool,
+    ) {
+        // FIXME: this is a brute-force approach.
+        for (handle, collider) in colliders.iter() {
+            if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) {
+                if !callback(handle, collider, inter) {
+                    return;
+                }
+            }
+        }
+    }
+}

src_testbed/physx_backend.rs

@@ -196,7 +196,8 @@ impl PhysxWorld {
                     }
                 } else {
                     physx_sys::PxShapeFlags {
-                        mBits: physx_sys::PxShapeFlag::eSIMULATION_SHAPE as u8,
+                        mBits: physx_sys::PxShapeFlag::eSIMULATION_SHAPE as u8
+                            | physx_sys::PxShapeFlag::eSCENE_QUERY_SHAPE as u8,
                     }
                 };
 

src_testbed/testbed.rs

@@ -21,9 +21,9 @@ use na::{self, Point2, Point3, Vector3};
 use rapier::dynamics::{
     ActivationStatus, IntegrationParameters, JointSet, RigidBodyHandle, RigidBodySet,
 };
-use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent};
+use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent, Ray};
 use rapier::math::Vector;
-use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline};
+use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline, QueryPipeline};
 #[cfg(feature = "fluids")]
 use salva::{coupling::ColliderCouplingSet, object::FluidHandle, LiquidWorld};
 
@@ -179,6 +179,7 @@ pub struct PhysicsState {
     pub colliders: ColliderSet,
     pub joints: JointSet,
     pub pipeline: PhysicsPipeline,
+    pub query_pipeline: QueryPipeline,
 }
 
 impl PhysicsState {
@@ -190,6 +191,7 @@ impl PhysicsState {
             colliders: ColliderSet::new(),
             joints: JointSet::new(),
             pipeline: PhysicsPipeline::new(),
+            query_pipeline: QueryPipeline::new(),
         }
     }
 }
@@ -197,6 +199,7 @@ impl PhysicsState {
 pub struct TestbedState {
     pub running: RunMode,
     pub draw_colls: bool,
+    pub highlighted_body: Option<RigidBodyHandle>,
     //    pub grabbed_object: Option<DefaultBodyPartHandle>,
     //    pub grabbed_object_constraint: Option<DefaultJointConstraintHandle>,
     pub grabbed_object_plane: (Point3<f32>, Vector3<f32>),
@@ -240,7 +243,7 @@ pub struct Testbed {
     hide_counters: bool,
     //    persistant_contacts: HashMap<ContactId, bool>,
     font: Rc<Font>,
-    // cursor_pos: Point2<f32>,
+    cursor_pos: Point2<f32>,
     events: PhysicsEvents,
     event_handler: ChannelEventCollector,
     ui: Option<TestbedUi>,
@@ -301,6 +304,7 @@ impl Testbed {
         let state = TestbedState {
             running: RunMode::Running,
             draw_colls: false,
+            highlighted_body: None,
             //            grabbed_object: None,
             //            grabbed_object_constraint: None,
             grabbed_object_plane: (Point3::origin(), na::zero()),
@@ -349,7 +353,7 @@ impl Testbed {
             hide_counters: true,
             //            persistant_contacts: HashMap::new(),
             font: Font::default(),
-            // cursor_pos: Point2::new(0.0f32, 0.0),
+            cursor_pos: Point2::new(0.0f32, 0.0),
             ui,
             event_handler,
             events,
@@ -408,6 +412,9 @@ impl Testbed {
             .set(TestbedActionFlags::RESET_WORLD_GRAPHICS, true);
         self.time = 0.0;
         self.state.timestep_id = 0;
+        self.state.highlighted_body = None;
+        self.physics.query_pipeline = QueryPipeline::new();
+        self.physics.pipeline = PhysicsPipeline::new();
         self.physics.pipeline.counters.enable();
 
         #[cfg(all(feature = "dim2", feature = "other-backends"))]
@@ -832,6 +839,10 @@ impl Testbed {
                     );
                 }
             }
+            WindowEvent::CursorPos(x, y, _) => {
+                self.cursor_pos.x = x as f32;
+                self.cursor_pos.y = y as f32;
+            }
             _ => {}
         }
 
@@ -1146,36 +1157,45 @@ impl Testbed {
                 self.state.grabbed_object = None;
                 self.state.grabbed_object_constraint = None;
             }
-            WindowEvent::CursorPos(x, y, modifiers) => {
+            _ => {}
-                self.cursor_pos.x = x as f32;
+        }
-                self.cursor_pos.y = y as f32;
+        */
+    }
+
+    #[cfg(feature = "dim2")]
+    fn highlight_hovered_body(&mut self, window: &Window) {
+        // Do nothing for now.
+    }
+
+    #[cfg(feature = "dim3")]
+    fn highlight_hovered_body(&mut self, window: &Window) {
+        if let Some(highlighted_body) = self.state.highlighted_body {
+            if let Some(nodes) = self.graphics.body_nodes_mut(highlighted_body) {
+                for node in nodes {
+                    node.unselect()
+                }
+            }
+        }
 
-                // update the joint
-                if let Some(joint) = self.state.grabbed_object_constraint {
         let size = window.size();
         let (pos, dir) = self
             .graphics
             .camera()
             .unproject(&self.cursor_pos, &na::convert(size));
-                    let (ref ppos, ref pdir) = self.state.grabbed_object_plane;
+        let ray = Ray::new(pos, dir);
+        let hit = self
+            .physics
+            .query_pipeline
+            .cast_ray(&self.physics.colliders, &ray, f32::MAX);
 
-                    if let Some(inter) = query::ray_toi_with_plane(ppos, pdir, &Ray::new(pos, dir))
+        if let Some((_, collider, _)) = hit {
-                    {
+            if self.physics.bodies[collider.parent()].is_dynamic() {
-                        let joint = self
+                self.state.highlighted_body = Some(collider.parent());
-                            .constraints
+                for node in self.graphics.body_nodes_mut(collider.parent()).unwrap() {
-                            .get_mut(joint)
+                    node.select()
-                            .unwrap()
-                            .downcast_mut::<MouseConstraint<f32, RigidBodyHandle>>()
-                            .unwrap();
-                        joint.set_anchor_1(pos + dir * inter)
                 }
             }
-
-                event.inhibited = modifiers.contains(Modifiers::Shift);
         }
-            _ => {}
-        }
-        */
     }
 }
 
@@ -1550,6 +1570,7 @@ impl State for Testbed {
             }
         }
 
+        self.highlight_hovered_body(window);
         self.graphics.draw(&self.physics.colliders, window);
 
         #[cfg(feature = "fluids")]