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.

3 files changed, 143 insertions, 5 deletions

diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 2d55857..183446b 100644
--- a/src/geometry/collider.rs
+++ b/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);
diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index 4f72778..5fcdf71 100644
--- a/src/geometry/mod.rs
+++ b/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;
diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs
index c3853bc..702b5aa 100644
--- a/src/geometry/waabb.rs
+++ b/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 }
+    }
+}