parry3d/query/ray/
ray_trimesh.rs

1use crate::math::Real;
2use crate::query::{Ray, RayCast, RayIntersection};
3use crate::shape::{CompositeShapeRef, TriMesh};
4
5#[cfg(feature = "dim3")]
6pub use ray_cast_with_culling::RayCullingMode;
7
8impl RayCast for TriMesh {
9    #[inline]
10    fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option<Real> {
11        CompositeShapeRef(self)
12            .cast_local_ray(ray, max_time_of_impact, solid)
13            .map(|hit| hit.1)
14    }
15
16    #[inline]
17    fn cast_local_ray_and_get_normal(
18        &self,
19        ray: &Ray,
20        max_time_of_impact: Real,
21        solid: bool,
22    ) -> Option<RayIntersection> {
23        CompositeShapeRef(self)
24            .cast_local_ray_and_get_normal(ray, max_time_of_impact, solid)
25            .map(|(triangle_id, hit)| hit.with_subshape(triangle_id))
26    }
27}
28
29// NOTE: implement the ray-cast with culling on its own submodule to facilitate feature gating.
30#[cfg(feature = "dim3")]
31mod ray_cast_with_culling {
32    use crate::math::{Pose, Real, Vector};
33    use crate::partitioning::Bvh;
34    use crate::query::details::NormalConstraints;
35    use crate::query::{Ray, RayIntersection};
36    use crate::shape::{
37        CompositeShape, CompositeShapeRef, Shape, TriMesh, Triangle, TypedCompositeShape,
38    };
39
40    /// Controls which side of a triangle a ray-cast is allowed to hit.
41    #[derive(Copy, Clone, PartialEq, Eq, Debug)]
42    pub enum RayCullingMode {
43        /// Ray-casting won’t hit back faces.
44        IgnoreBackfaces,
45        /// Ray-casting won’t hit front faces.
46        IgnoreFrontfaces,
47    }
48
49    impl RayCullingMode {
50        fn check(self, tri_normal: Vector, ray_dir: Vector) -> bool {
51            match self {
52                RayCullingMode::IgnoreBackfaces => tri_normal.dot(ray_dir) < 0.0,
53                RayCullingMode::IgnoreFrontfaces => tri_normal.dot(ray_dir) > 0.0,
54            }
55        }
56    }
57
58    /// A utility shape with a `TypedCompositeShape` implementation that skips triangles that
59    /// are back-faces or front-faces relative to a given ray and culling mode.
60    struct TriMeshWithCulling<'a> {
61        trimesh: &'a TriMesh,
62        culling: RayCullingMode,
63        ray: &'a Ray,
64    }
65
66    impl CompositeShape for TriMeshWithCulling<'_> {
67        fn map_part_at(
68            &self,
69            shape_id: u32,
70            f: &mut dyn FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>),
71        ) {
72            let _ = self.map_untyped_part_at(shape_id, f);
73        }
74
75        /// Gets the acceleration structure of the composite shape.
76        fn bvh(&self) -> &Bvh {
77            self.trimesh.bvh()
78        }
79    }
80
81    impl TypedCompositeShape for TriMeshWithCulling<'_> {
82        type PartShape = Triangle;
83        type PartNormalConstraints = ();
84
85        #[inline(always)]
86        fn map_typed_part_at<T>(
87            &self,
88            i: u32,
89            mut f: impl FnMut(
90                Option<&Pose>,
91                &Self::PartShape,
92                Option<&Self::PartNormalConstraints>,
93            ) -> T,
94        ) -> Option<T> {
95            let tri = self.trimesh.triangle(i);
96            let tri_normal = tri.scaled_normal();
97
98            if self.culling.check(tri_normal, self.ray.dir) {
99                Some(f(None, &tri, None))
100            } else {
101                None
102            }
103        }
104
105        #[inline(always)]
106        fn map_untyped_part_at<T>(
107            &self,
108            i: u32,
109            mut f: impl FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>) -> T,
110        ) -> Option<T> {
111            let tri = self.trimesh.triangle(i);
112            let tri_normal = tri.scaled_normal();
113
114            if self.culling.check(tri_normal, self.ray.dir) {
115                Some(f(None, &tri, None))
116            } else {
117                None
118            }
119        }
120    }
121
122    impl TriMesh {
123        /// Casts a ray on this triangle mesh transformed by `m`.
124        ///
125        /// Hits happening later than `max_time_of_impact` are ignored. In other words, hits are
126        /// only searched along the segment `[ray.origin, ray.origin + ray.dir * max_time_of_impact`].
127        ///
128        /// Hits on back-faces or front-faces are ignored, depending on the given `culling` mode.
129        #[inline]
130        pub fn cast_ray_with_culling(
131            &self,
132            m: &Pose,
133            ray: &Ray,
134            max_time_of_impact: Real,
135            culling: RayCullingMode,
136        ) -> Option<RayIntersection> {
137            let ls_ray = ray.inverse_transform_by(m);
138            self.cast_local_ray_with_culling(&ls_ray, max_time_of_impact, culling)
139                .map(|inter| inter.transform_by(m))
140        }
141
142        /// Casts a ray on this triangle mesh.
143        ///
144        /// This is the same as [`TriMesh::cast_ray_with_culling`] except that the ray is given
145        /// in the mesh’s local-space.
146        pub fn cast_local_ray_with_culling(
147            &self,
148            ray: &Ray,
149            max_time_of_impact: Real,
150            culling: RayCullingMode,
151        ) -> Option<RayIntersection> {
152            let mesh_with_culling = TriMeshWithCulling {
153                trimesh: self,
154                culling,
155                ray,
156            };
157            CompositeShapeRef(&mesh_with_culling)
158                .cast_local_ray_and_get_normal(ray, max_time_of_impact, false)
159                .map(|(triangle_id, hit)| hit.with_subshape(triangle_id))
160        }
161    }
162
163    #[cfg(test)]
164    mod test {
165        use crate::math::Vector;
166        use crate::query::{Ray, RayCullingMode};
167        use crate::shape::TriMesh;
168
169        #[test]
170        fn cast_ray_on_trimesh_with_culling() {
171            let vertices = vec![
172                Vector::ZERO,
173                Vector::new(1.0, 0.0, 0.0),
174                Vector::new(0.0, 1.0, 0.0),
175            ];
176            let indices = vec![[0, 1, 2]];
177            let ray_up = Ray::new(Vector::new(0.0, 0.0, -1.0), Vector::new(0.0, 0.0, 1.0));
178            let ray_down = Ray::new(Vector::new(0.0, 0.0, 1.0), Vector::new(0.0, 0.0, -1.0));
179
180            let mesh = TriMesh::new(vertices, indices).unwrap();
181            assert!(mesh
182                .cast_local_ray_with_culling(&ray_up, 1000.0, RayCullingMode::IgnoreFrontfaces)
183                .is_some());
184            assert!(mesh
185                .cast_local_ray_with_culling(&ray_down, 1000.0, RayCullingMode::IgnoreFrontfaces)
186                .is_none());
187
188            assert!(mesh
189                .cast_local_ray_with_culling(&ray_up, 1000.0, RayCullingMode::IgnoreBackfaces)
190                .is_none());
191            assert!(mesh
192                .cast_local_ray_with_culling(&ray_down, 1000.0, RayCullingMode::IgnoreBackfaces)
193                .is_some());
194        }
195    }
196}
parry3d/query/ray/ray_trimesh.rs

parry3d/query/ray/
ray_trimesh.rs
