shape casting

- fixes #590
- fixed clippy warnings

src/scene/collider.rs

@@ -341,6 +341,73 @@ impl From<geometry::InteractionGroups> for InteractionGroups {
     }
 }
 
+bitflags::bitflags! {
+    #[derive(Default, Copy, Clone)]
+    /// Flags for excluding whole sets of colliders from a scene query.
+    pub struct QueryFilterFlags: u32 {
+        /// Exclude from the query any collider attached to a fixed rigid-body and colliders with no rigid-body attached.
+        const EXCLUDE_FIXED = 1 << 1;
+        /// Exclude from the query any collider attached to a kinematic rigid-body.
+        const EXCLUDE_KINEMATIC = 1 << 2;
+        /// Exclude from the query any collider attached to a dynamic rigid-body.
+        const EXCLUDE_DYNAMIC = 1 << 3;
+        /// Exclude from the query any collider that is a sensor.
+        const EXCLUDE_SENSORS = 1 << 4;
+        /// Exclude from the query any collider that is not a sensor.
+        const EXCLUDE_SOLIDS = 1 << 5;
+        /// Excludes all colliders not attached to a dynamic rigid-body.
+        const ONLY_DYNAMIC = Self::EXCLUDE_FIXED.bits() | Self::EXCLUDE_KINEMATIC.bits();
+        /// Excludes all colliders not attached to a kinematic rigid-body.
+        const ONLY_KINEMATIC = Self::EXCLUDE_DYNAMIC.bits() | Self::EXCLUDE_FIXED.bits();
+        /// Exclude all colliders attached to a non-fixed rigid-body
+        /// (this will not exclude colliders not attached to any rigid-body).
+        const ONLY_FIXED = Self::EXCLUDE_DYNAMIC.bits() | Self::EXCLUDE_KINEMATIC.bits();
+    }
+}
+
+/// The status of the time-of-impact computation algorithm.
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+pub enum TOIStatus {
+    /// The TOI algorithm ran out of iterations before achieving convergence.
+    ///
+    /// The content of the `TOI` will still be a conservative approximation of the actual result so
+    /// it is often fine to interpret this case as a success.
+    OutOfIterations,
+    /// The TOI algorithm converged successfully.
+    Converged,
+    /// Something went wrong during the TOI computation, likely due to numerical instabilities.
+    ///
+    /// The content of the `TOI` will still be a conservative approximation of the actual result so
+    /// it is often fine to interpret this case as a success.
+    Failed,
+    /// The two shape already overlap at the time 0.
+    ///
+    /// The witness points and normals provided by the `TOI` will have undefined values.
+    Penetrating,
+}
+
+impl From<rapier3d::parry::query::TOIStatus> for TOIStatus {
+    fn from(value: rapier3d::parry::query::TOIStatus) -> Self {
+        match value {
+            rapier3d::parry::query::TOIStatus::OutOfIterations => Self::OutOfIterations,
+            rapier3d::parry::query::TOIStatus::Converged => Self::Converged,
+            rapier3d::parry::query::TOIStatus::Failed => Self::Failed,
+            rapier3d::parry::query::TOIStatus::Penetrating => Self::Penetrating,
+        }
+    }
+}
+
+impl From<rapier2d::parry::query::TOIStatus> for TOIStatus {
+    fn from(value: rapier2d::parry::query::TOIStatus) -> Self {
+        match value {
+            rapier2d::parry::query::TOIStatus::OutOfIterations => Self::OutOfIterations,
+            rapier2d::parry::query::TOIStatus::Converged => Self::Converged,
+            rapier2d::parry::query::TOIStatus::Failed => Self::Failed,
+            rapier2d::parry::query::TOIStatus::Penetrating => Self::Penetrating,
+        }
+    }
+}
+
 /// Possible collider shapes.
 #[derive(Clone, Debug, PartialEq, Visit, Reflect, AsRefStr, EnumString, VariantNames)]
 pub enum ColliderShape {

src/scene/dim2/physics.rs

@@ -1,11 +1,12 @@
 //! Scene physics module.
 
 use crate::scene::dim2::joint::JointLocalFrames;
+use crate::scene::graph::Graph;
 use crate::{
     core::{
         algebra::{
             Isometry2, Isometry3, Matrix4, Point2, Rotation3, Translation2, Translation3,
-            UnitComplex, UnitQuaternion, Vector2, Vector3,
+            UnitComplex, UnitQuaternion, UnitVector2, Vector2, Vector3,
         },
         arrayvec::ArrayVec,
         instant,
@@ -42,7 +43,7 @@ use rapier2d::{
         BroadPhase, Collider, ColliderBuilder, ColliderHandle, ColliderSet, Cuboid,
         InteractionGroups, NarrowPhase, Ray, SharedShape,
     },
-    pipeline::{DebugRenderPipeline, EventHandler, PhysicsPipeline, QueryFilter, QueryPipeline},
+    pipeline::{DebugRenderPipeline, EventHandler, PhysicsPipeline, QueryPipeline},
 };
 use std::num::NonZeroUsize;
 use std::{
@@ -53,6 +54,9 @@ use std::{
     sync::Arc,
 };
 
+use fyrox_graph::{BaseSceneGraph, SceneGraphNode};
+pub use rapier2d::geometry::shape::*;
+
 /// A trait for ray cast results storage. It has two implementations: Vec and ArrayVec.
 /// Latter is needed for the cases where you need to avoid runtime memory allocations
 /// and do everything on stack.
@@ -431,6 +435,48 @@ fn u32_to_group(v: u32) -> rapier2d::geometry::Group {
     rapier2d::geometry::Group::from_bits(v).unwrap_or_else(rapier2d::geometry::Group::all)
 }
 
+/// A filter tha describes what collider should be included or excluded from a scene query.
+#[derive(Copy, Clone, Default)]
+#[allow(clippy::type_complexity)]
+pub struct QueryFilter<'a> {
+    /// Flags indicating what particular type of colliders should be excluded from the scene query.
+    pub flags: collider::QueryFilterFlags,
+    /// If set, only colliders with collision groups compatible with this one will
+    /// be included in the scene query.
+    pub groups: Option<collider::InteractionGroups>,
+    /// If set, this collider will be excluded from the scene query.
+    pub exclude_collider: Option<Handle<Node>>,
+    /// If set, any collider attached to this rigid-body will be excluded from the scene query.
+    pub exclude_rigid_body: Option<Handle<Node>>,
+    /// If set, any collider for which this closure returns false will be excluded from the scene query.
+    pub predicate: Option<&'a dyn Fn(Handle<Node>, &collider::Collider) -> bool>,
+}
+
+/// The result of a time-of-impact (TOI) computation.
+#[derive(Copy, Clone, Debug)]
+pub struct TOI {
+    /// The time at which the objects touch.
+    pub toi: f32,
+    /// The local-space closest point on the first shape at the time of impact.
+    ///
+    /// Undefined if `status` is `Penetrating`.
+    pub witness1: Point2<f32>,
+    /// The local-space closest point on the second shape at the time of impact.
+    ///
+    /// Undefined if `status` is `Penetrating`.
+    pub witness2: Point2<f32>,
+    /// The local-space outward normal on the first shape at the time of impact.
+    ///
+    /// Undefined if `status` is `Penetrating`.
+    pub normal1: UnitVector2<f32>,
+    /// The local-space outward normal on the second shape at the time of impact.
+    ///
+    /// Undefined if `status` is `Penetrating`.
+    pub normal2: UnitVector2<f32>,
+    /// The way the time-of-impact computation algorithm terminated.
+    pub status: collider::TOIStatus,
+}
+
 impl PhysicsWorld {
     /// Creates a new instance of the physics world.
     pub(crate) fn new() -> Self {
@@ -600,7 +646,7 @@ impl PhysicsWorld {
             &ray,
             opts.max_len,
             true,
-            QueryFilter::new().groups(InteractionGroups::new(
+            rapier2d::pipeline::QueryFilter::new().groups(InteractionGroups::new(
                 u32_to_group(opts.groups.memberships.0),
                 u32_to_group(opts.groups.filter.0),
             )),
@@ -634,6 +680,93 @@ impl PhysicsWorld {
         );
     }
 
+    /// Casts a shape at a constant linear velocity and retrieve the first collider it hits.
+    ///
+    /// This is similar to ray-casting except that we are casting a whole shape instead of just a
+    /// point (the ray origin). In the resulting `TOI`, witness and normal 1 refer to the world
+    /// collider, and are in world space.
+    ///
+    /// # Parameters
+    ///
+    /// * `graph` - a reference to the scene graph.
+    /// * `shape` - The shape to cast.
+    /// * `shape_pos` - The initial position of the shape to cast.
+    /// * `shape_vel` - The constant velocity of the shape to cast (i.e. the cast direction).
+    /// * `max_toi` - The maximum time-of-impact that can be reported by this cast. This effectively
+    ///   limits the distance traveled by the shape to `shapeVel.norm() * maxToi`.
+    /// * `stop_at_penetration` - If set to `false`, the linear shape-cast won’t immediately stop if
+    ///   the shape is penetrating another shape at its starting point **and** its trajectory is such
+    ///   that it’s on a path to exist that penetration state.
+    /// * `filter`: set of rules used to determine which collider is taken into account by this scene
+    ///   query.
+    pub fn cast_shape(
+        &self,
+        graph: &Graph,
+        shape: &dyn Shape,
+        shape_pos: &Isometry2<f32>,
+        shape_vel: &Vector2<f32>,
+        max_toi: f32,
+        stop_at_penetration: bool,
+        filter: QueryFilter,
+    ) -> Option<(Handle<Node>, TOI)> {
+        let predicate = |handle: ColliderHandle, _: &Collider| -> bool {
+            if let Some(pred) = filter.predicate {
+                let h = Handle::decode_from_u128(self.colliders.get(handle).unwrap().user_data);
+                pred(
+                    h,
+                    graph.node(h).component_ref::<collider::Collider>().unwrap(),
+                )
+            } else {
+                true
+            }
+        };
+
+        let filter = rapier2d::pipeline::QueryFilter {
+            flags: rapier2d::pipeline::QueryFilterFlags::from_bits(filter.flags.bits()).unwrap(),
+            groups: filter.groups.map(|g| {
+                InteractionGroups::new(u32_to_group(g.memberships.0), u32_to_group(g.filter.0))
+            }),
+            exclude_collider: filter
+                .exclude_collider
+                .and_then(|h| graph.try_get(h))
+                .and_then(|n| n.component_ref::<dim2::collider::Collider>())
+                .map(|c| c.native.get()),
+            exclude_rigid_body: filter
+                .exclude_collider
+                .and_then(|h| graph.try_get(h))
+                .and_then(|n| n.component_ref::<dim2::rigidbody::RigidBody>())
+                .map(|c| c.native.get()),
+            predicate: Some(&predicate),
+        };
+
+        let query = self.query.borrow_mut();
+
+        query
+            .cast_shape(
+                &self.bodies,
+                &self.colliders,
+                shape_pos,
+                shape_vel,
+                shape,
+                max_toi,
+                stop_at_penetration,
+                filter,
+            )
+            .map(|(handle, toi)| {
+                (
+                    Handle::decode_from_u128(self.colliders.get(handle).unwrap().user_data),
+                    TOI {
+                        toi: toi.toi,
+                        witness1: toi.witness1,
+                        witness2: toi.witness2,
+                        normal1: toi.normal1,
+                        normal2: toi.normal2,
+                        status: toi.status.into(),
+                    },
+                )
+            })
+    }
+
     pub(crate) fn set_rigid_body_position(
         &mut self,
         rigid_body: &scene::dim2::rigidbody::RigidBody,