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
97struct 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 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 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}