ncollide3d/query/ray/
ray_trimesh.rs

1use crate::bounding_volume::AABB;
2use crate::math::Isometry;
3use crate::partitioning::{BestFirstVisitStatus, BestFirstVisitor};
4use crate::query::{self, Ray, RayCast, RayIntersection};
5use crate::shape::{CompositeShape, FeatureId, TriMesh};
6use na::{Point2, RealField, Vector3};
7
8impl<N: RealField + Copy> RayCast<N> for TriMesh<N> {
9    #[inline]
10    fn toi_with_ray(&self, m: &Isometry<N>, ray: &Ray<N>, max_toi: N, _: bool) -> Option<N> {
11        let ls_ray = ray.inverse_transform_by(m);
12
13        let mut visitor = TriMeshRayToiVisitor {
14            mesh: self,
15            ray: &ls_ray,
16            max_toi,
17        };
18
19        self.bvh().best_first_search(&mut visitor).map(|res| res.1)
20    }
21
22    #[inline]
23    fn toi_and_normal_with_ray(
24        &self,
25        m: &Isometry<N>,
26        ray: &Ray<N>,
27        max_toi: N,
28        _: bool,
29    ) -> Option<RayIntersection<N>> {
30        let ls_ray = ray.inverse_transform_by(m);
31
32        let mut visitor = TriMeshRayToiAndNormalVisitor {
33            mesh: self,
34            ray: &ls_ray,
35            max_toi,
36        };
37
38        self.bvh()
39            .best_first_search(&mut visitor)
40            .map(|(_, (best, mut res))| {
41                if let FeatureId::Face(1) = res.feature {
42                    res.feature = FeatureId::Face(best + self.faces().len());
43                } else {
44                    res.feature = FeatureId::Face(best);
45                }
46
47                res.normal = m * res.normal;
48                res
49            })
50    }
51
52    fn toi_and_normal_and_uv_with_ray(
53        &self,
54        m: &Isometry<N>,
55        ray: &Ray<N>,
56        max_toi: N,
57        solid: bool,
58    ) -> Option<RayIntersection<N>> {
59        if self.uvs().is_none() {
60            return self.toi_and_normal_with_ray(m, ray, max_toi, solid);
61        }
62
63        let ls_ray = ray.inverse_transform_by(m);
64
65        let mut visitor = TriMeshRayToiAndNormalAndUVsVisitor {
66            mesh: self,
67            ray: &ls_ray,
68            max_toi,
69        };
70        let cast = self.bvh().best_first_search(&mut visitor);
71
72        cast.map(|(_, (best, inter, uv))| {
73            let toi = inter.toi;
74            let n = inter.normal;
75
76            let idx = &self.faces()[best].indices;
77            let uvs = self.uvs().unwrap();
78
79            let uv1 = uvs[idx[0]];
80            let uv2 = uvs[idx[1]];
81            let uv3 = uvs[idx[2]];
82
83            let uvx = uv1.x * uv.x + uv2.x * uv.y + uv3.x * uv.z;
84            let uvy = uv1.y * uv.x + uv2.y * uv.y + uv3.y * uv.z;
85
86            let feature = if let FeatureId::Face(1) = inter.feature {
87                FeatureId::Face(best + self.faces().len())
88            } else {
89                FeatureId::Face(best)
90            };
91
92            RayIntersection::new_with_uvs(toi, m * n, feature, Some(Point2::new(uvx, uvy)))
93        })
94    }
95}
96
97/*
98 * Costs functions.
99 */
100struct TriMeshRayToiVisitor<'a, N: 'a + RealField + Copy> {
101    mesh: &'a TriMesh<N>,
102    ray: &'a Ray<N>,
103    max_toi: N,
104}
105
106impl<'a, N: RealField + Copy> BestFirstVisitor<N, usize, AABB<N>> for TriMeshRayToiVisitor<'a, N> {
107    type Result = N;
108
109    #[inline]
110    fn visit(
111        &mut self,
112        best: N,
113        aabb: &AABB<N>,
114        data: Option<&usize>,
115    ) -> BestFirstVisitStatus<N, Self::Result> {
116        if let Some(toi) = aabb.toi_with_ray(&Isometry::identity(), self.ray, self.max_toi, true) {
117            let mut res = BestFirstVisitStatus::Continue {
118                cost: toi,
119                result: None,
120            };
121
122            if let Some(b) = data {
123                if toi < best {
124                    // FIXME: optimize this by not using Isometry identity.
125                    let triangle = self.mesh.triangle_at(*b);
126                    if let Some(toi) =
127                        triangle.toi_with_ray(&Isometry::identity(), self.ray, self.max_toi, true)
128                    {
129                        res = BestFirstVisitStatus::Continue {
130                            cost: toi,
131                            result: Some(toi),
132                        }
133                    }
134                }
135            }
136
137            res
138        } else {
139            BestFirstVisitStatus::Stop
140        }
141    }
142}
143
144struct TriMeshRayToiAndNormalVisitor<'a, N: 'a + RealField + Copy> {
145    mesh: &'a TriMesh<N>,
146    ray: &'a Ray<N>,
147    max_toi: N,
148}
149
150impl<'a, N: RealField + Copy> BestFirstVisitor<N, usize, AABB<N>>
151    for TriMeshRayToiAndNormalVisitor<'a, N>
152{
153    type Result = (usize, RayIntersection<N>);
154
155    #[inline]
156    fn visit(
157        &mut self,
158        best: N,
159        aabb: &AABB<N>,
160        data: Option<&usize>,
161    ) -> BestFirstVisitStatus<N, Self::Result> {
162        if let Some(toi) = aabb.toi_with_ray(&Isometry::identity(), self.ray, self.max_toi, true) {
163            let mut res = BestFirstVisitStatus::Continue {
164                cost: toi,
165                result: None,
166            };
167
168            if let Some(b) = data {
169                if toi < best {
170                    // FIXME: optimize this by not using Isometry identity.
171                    let triangle = self.mesh.triangle_at(*b);
172                    if let Some(toi) = triangle.toi_and_normal_with_ray(
173                        &Isometry::identity(),
174                        self.ray,
175                        self.max_toi,
176                        true,
177                    ) {
178                        res = BestFirstVisitStatus::Continue {
179                            cost: toi.toi,
180                            result: Some((*b, toi)),
181                        };
182                    }
183                }
184            }
185
186            res
187        } else {
188            BestFirstVisitStatus::Stop
189        }
190    }
191}
192
193struct TriMeshRayToiAndNormalAndUVsVisitor<'a, N: 'a + RealField + Copy> {
194    mesh: &'a TriMesh<N>,
195    ray: &'a Ray<N>,
196    max_toi: N,
197}
198
199impl<'a, N: RealField + Copy> BestFirstVisitor<N, usize, AABB<N>>
200    for TriMeshRayToiAndNormalAndUVsVisitor<'a, N>
201{
202    type Result = (usize, RayIntersection<N>, Vector3<N>);
203
204    #[inline]
205    fn visit(
206        &mut self,
207        best: N,
208        aabb: &AABB<N>,
209        data: Option<&usize>,
210    ) -> BestFirstVisitStatus<N, Self::Result> {
211        if let Some(toi) = aabb.toi_with_ray(&Isometry::identity(), self.ray, self.max_toi, true) {
212            let mut res = BestFirstVisitStatus::Continue {
213                cost: toi,
214                result: None,
215            };
216
217            if let Some(i) = data {
218                if toi < best {
219                    let vs = self.mesh.points();
220                    let idx = self.mesh.faces()[*i].indices;
221
222                    let a = &vs[idx[0]];
223                    let b = &vs[idx[1]];
224                    let c = &vs[idx[2]];
225
226                    if let Some(inter) = query::ray_intersection_with_triangle(a, b, c, self.ray) {
227                        if inter.0.toi <= self.max_toi {
228                            res = BestFirstVisitStatus::Continue {
229                                cost: inter.0.toi,
230                                result: Some((*i, inter.0, inter.1)),
231                            };
232                        }
233                    }
234                }
235            }
236
237            res
238        } else {
239            BestFirstVisitStatus::Stop
240        }
241    }
242}