1use crate::math::{Isometry, Point, Vector};
2use crate::query::{PointProjection, PointQuery, PointQueryWithLocation};
3use crate::shape::{FeatureId, Tetrahedron, TetrahedronPointLocation};
4use na::{self, RealField};
5
6impl<N: RealField + Copy> PointQuery<N> for Tetrahedron<N> {
7 #[inline]
8 fn project_point(&self, m: &Isometry<N>, pt: &Point<N>, solid: bool) -> PointProjection<N> {
9 let (projection, _) = self.project_point_with_location(m, pt, solid);
10 projection
11 }
12
13 #[inline]
14 fn project_point_with_feature(
15 &self,
16 m: &Isometry<N>,
17 pt: &Point<N>,
18 ) -> (PointProjection<N>, FeatureId) {
19 let (proj, loc) = self.project_point_with_location(m, pt, false);
20 let feature = match loc {
21 TetrahedronPointLocation::OnVertex(i) => FeatureId::Vertex(i),
22 TetrahedronPointLocation::OnEdge(i, _) => FeatureId::Edge(i),
23 TetrahedronPointLocation::OnFace(i, _) => FeatureId::Face(i),
24 TetrahedronPointLocation::OnSolid => unreachable!(),
25 };
26
27 (proj, feature)
28 }
29}
30
31impl<N: RealField + Copy> PointQueryWithLocation<N> for Tetrahedron<N> {
32 type Location = TetrahedronPointLocation<N>;
33
34 #[inline]
35 fn project_point_with_location(
36 &self,
37 m: &Isometry<N>,
38 pt: &Point<N>,
39 solid: bool,
40 ) -> (PointProjection<N>, Self::Location) {
41 let p = m.inverse_transform_point(pt);
42
43 let ab = self.b - self.a;
44 let ac = self.c - self.a;
45 let ad = self.d - self.a;
46 let ap = p - self.a;
47
48 let ap_ab = ap.dot(&ab);
52 let ap_ac = ap.dot(&ac);
53 let ap_ad = ap.dot(&ad);
54
55 let _0: N = na::zero();
56
57 if ap_ab <= _0 && ap_ac <= _0 && ap_ad <= _0 {
58 let proj = PointProjection::new(false, m * self.a);
60 return (proj, TetrahedronPointLocation::OnVertex(0));
61 }
62
63 let bc = self.c - self.b;
64 let bd = self.d - self.b;
65 let bp = p - self.b;
66
67 let bp_bc = bp.dot(&bc);
68 let bp_bd = bp.dot(&bd);
69 let bp_ab = bp.dot(&ab);
70
71 if bp_bc <= _0 && bp_bd <= _0 && bp_ab >= _0 {
72 let proj = PointProjection::new(false, m * self.b);
74 return (proj, TetrahedronPointLocation::OnVertex(1));
75 }
76
77 let cd = self.d - self.c;
78 let cp = p - self.c;
79
80 let cp_ac = cp.dot(&ac);
81 let cp_bc = cp.dot(&bc);
82 let cp_cd = cp.dot(&cd);
83
84 if cp_cd <= _0 && cp_bc >= _0 && cp_ac >= _0 {
85 let proj = PointProjection::new(false, m * self.c);
87 return (proj, TetrahedronPointLocation::OnVertex(2));
88 }
89
90 let dp = p - self.d;
91
92 let dp_cd = dp.dot(&cd);
93 let dp_bd = dp.dot(&bd);
94 let dp_ad = dp.dot(&ad);
95
96 if dp_ad >= _0 && dp_bd >= _0 && dp_cd >= _0 {
97 let proj = PointProjection::new(false, m * self.d);
99 return (proj, TetrahedronPointLocation::OnVertex(3));
100 }
101
102 #[inline(always)]
106 fn check_edge<N: RealField + Copy>(
107 i: usize,
108 m: &Isometry<N>,
109 a: &Point<N>,
110 _: &Point<N>,
111 nabc: &Vector<N>,
112 nabd: &Vector<N>,
113 ap: &Vector<N>,
114 ab: &Vector<N>,
115 ap_ab: N, bp_ab: N, ) -> (
118 N,
119 N,
120 Option<(PointProjection<N>, TetrahedronPointLocation<N>)>,
121 ) {
122 let _0: N = na::zero();
123 let _1: N = na::one();
124
125 let ab_ab = ap_ab - bp_ab;
126
127 let ap_x_ab = ap.cross(ab);
134 let dabc = ap_x_ab.dot(nabc);
135 let dabd = ap_x_ab.dot(nabd);
136
137 if ab_ab != _0 && dabc >= _0 && dabd >= _0 && ap_ab >= _0 && ap_ab <= ab_ab {
139 let u = ap_ab / ab_ab;
141 let bcoords = [_1 - u, u];
142 let res = a + ab * u;
143 let proj = PointProjection::new(false, m * res);
144 (
145 dabc,
146 dabd,
147 Some((proj, TetrahedronPointLocation::OnEdge(i, bcoords))),
148 )
149 } else {
150 (dabc, dabd, None)
151 }
152 }
153
154 let nabc = ab.cross(&ac);
158 let nabd = ab.cross(&ad);
159 let (dabc, dabd, res) = check_edge(
160 0, m, &self.a, &self.b, &nabc, &nabd, &ap, &ab, ap_ab,
161 bp_ab, );
163 if let Some(res) = res {
164 return res;
165 }
166
167 let nacd = ac.cross(&ad);
175 let (dacd, dacb, res) = check_edge(
176 1, m, &self.a, &self.c, &nacd, &-nabc, &ap, &ac, ap_ac,
177 cp_ac, );
179 if let Some(res) = res {
180 return res;
181 }
182
183 let (dadb, dadc, res) = check_edge(
191 2, m, &self.a, &self.d, &-nabd, &-nacd, &ap, &ad, ap_ad,
192 dp_ad, );
194 if let Some(res) = res {
195 return res;
196 }
197
198 let nbcd = bc.cross(&bd);
205 let (dbca, dbcd, res) = check_edge(
207 3, m, &self.b, &self.c, &nabc, &nbcd, &bp, &bc, bp_bc,
208 cp_bc, );
210 if let Some(res) = res {
211 return res;
212 }
213
214 let (dbdc, dbda, res) = check_edge(
224 4, m, &self.b, &self.d, &-nbcd, &nabd, &bp, &bd, bp_bd,
225 dp_bd, );
227 if let Some(res) = res {
228 return res;
229 }
230
231 let (dcda, dcdb, res) = check_edge(
240 5, m, &self.c, &self.d, &nacd, &nbcd, &cp, &cd, cp_cd,
241 dp_cd, );
243 if let Some(res) = res {
244 return res;
245 }
246
247 #[inline(always)]
251 fn check_face<N: RealField + Copy>(
252 i: usize,
253 a: &Point<N>,
254 b: &Point<N>,
255 c: &Point<N>,
256 m: &Isometry<N>,
257 ap: &Vector<N>,
258 bp: &Vector<N>,
259 cp: &Vector<N>,
260 ab: &Vector<N>,
261 ac: &Vector<N>,
262 ad: &Vector<N>,
263 dabc: N,
264 dbca: N,
265 dacb: N,
266 ) -> Option<(PointProjection<N>, TetrahedronPointLocation<N>)> {
269 let _0: N = na::zero();
270 let _1: N = na::one();
271
272 if dabc < _0 && dbca < _0 && dacb < _0 {
273 let n = ab.cross(ac); if n.dot(ad) * n.dot(ap) < _0 {
275 let normal = n.try_normalize(N::default_epsilon())?;
290 let vc = normal.dot(&ap.cross(bp));
291 let va = normal.dot(&bp.cross(cp));
292 let vb = normal.dot(&cp.cross(ap));
293
294 let denom = va + vb + vc;
295 assert!(denom != _0);
296 let inv_denom = _1 / denom;
297
298 let bcoords = [va * inv_denom, vb * inv_denom, vc * inv_denom];
299 let res = a * bcoords[0] + b.coords * bcoords[1] + c.coords * bcoords[2];
300 let proj = PointProjection::new(false, m * res);
301
302 return Some((proj, TetrahedronPointLocation::OnFace(i, bcoords)));
303 }
304 }
305 return None;
306 }
307
308 if let Some(res) = check_face(
310 0, &self.a, &self.b, &self.c, m, &ap, &bp, &cp, &ab, &ac, &ad, dabc, dbca,
311 dacb,
312 ) {
315 return res;
316 }
317
318 if let Some(res) = check_face(
320 1, &self.a, &self.b, &self.d, m, &ap, &bp, &dp, &ab, &ad, &ac, dadb, dabd,
321 dbda,
322 ) {
325 return res;
326 }
327 if let Some(res) = check_face(
329 2, &self.a, &self.c, &self.d, m, &ap, &cp, &dp, &ac, &ad, &ab, dacd, dcda,
330 dadc,
331 ) {
334 return res;
335 }
336 if let Some(res) = check_face(
338 3, &self.b, &self.c, &self.d, m, &bp, &cp, &dp, &bc, &bd, &-ab, dbcd, dcdb,
339 dbdc,
340 ) {
343 return res;
344 }
345
346 if !solid {
347 unimplemented!("Non-solid ray-cast on a tetrahedron is not yet implemented.")
349 }
350
351 let proj = PointProjection::new(true, m * p);
352 return (proj, TetrahedronPointLocation::OnSolid);
353 }
354}