1use crate::math::{Point, Real, Vector};
2use crate::query::{PointProjection, PointQuery, PointQueryWithLocation};
3use crate::shape::{FeatureId, Tetrahedron, TetrahedronPointLocation};
4
5impl PointQuery for Tetrahedron {
6 #[inline]
7 fn project_local_point(&self, pt: &Point<Real>, solid: bool) -> PointProjection {
8 self.project_local_point_and_get_location(pt, solid).0
9 }
10
11 #[inline]
12 fn project_local_point_and_get_feature(
13 &self,
14 pt: &Point<Real>,
15 ) -> (PointProjection, FeatureId) {
16 let (proj, loc) = self.project_local_point_and_get_location(pt, false);
17 let feature = match loc {
18 TetrahedronPointLocation::OnVertex(i) => FeatureId::Vertex(i),
19 TetrahedronPointLocation::OnEdge(i, _) => FeatureId::Edge(i),
20 TetrahedronPointLocation::OnFace(i, _) => FeatureId::Face(i),
21 TetrahedronPointLocation::OnSolid => unreachable!(),
22 };
23
24 (proj, feature)
25 }
26}
27
28impl PointQueryWithLocation for Tetrahedron {
29 type Location = TetrahedronPointLocation;
30
31 #[inline]
32 fn project_local_point_and_get_location(
33 &self,
34 pt: &Point<Real>,
35 solid: bool,
36 ) -> (PointProjection, Self::Location) {
37 let ab = self.b - self.a;
38 let ac = self.c - self.a;
39 let ad = self.d - self.a;
40 let ap = pt - self.a;
41
42 let ap_ab = ap.dot(&ab);
46 let ap_ac = ap.dot(&ac);
47 let ap_ad = ap.dot(&ad);
48
49 if ap_ab <= 0.0 && ap_ac <= 0.0 && ap_ad <= 0.0 {
50 let proj = PointProjection::new(false, self.a);
52 return (proj, TetrahedronPointLocation::OnVertex(0));
53 }
54
55 let bc = self.c - self.b;
56 let bd = self.d - self.b;
57 let bp = pt - self.b;
58
59 let bp_bc = bp.dot(&bc);
60 let bp_bd = bp.dot(&bd);
61 let bp_ab = bp.dot(&ab);
62
63 if bp_bc <= 0.0 && bp_bd <= 0.0 && bp_ab >= 0.0 {
64 let proj = PointProjection::new(false, self.b);
66 return (proj, TetrahedronPointLocation::OnVertex(1));
67 }
68
69 let cd = self.d - self.c;
70 let cp = pt - self.c;
71
72 let cp_ac = cp.dot(&ac);
73 let cp_bc = cp.dot(&bc);
74 let cp_cd = cp.dot(&cd);
75
76 if cp_cd <= 0.0 && cp_bc >= 0.0 && cp_ac >= 0.0 {
77 let proj = PointProjection::new(false, self.c);
79 return (proj, TetrahedronPointLocation::OnVertex(2));
80 }
81
82 let dp = pt - self.d;
83
84 let dp_cd = dp.dot(&cd);
85 let dp_bd = dp.dot(&bd);
86 let dp_ad = dp.dot(&ad);
87
88 if dp_ad >= 0.0 && dp_bd >= 0.0 && dp_cd >= 0.0 {
89 let proj = PointProjection::new(false, self.d);
91 return (proj, TetrahedronPointLocation::OnVertex(3));
92 }
93
94 #[inline(always)]
98 fn check_edge(
99 i: usize,
100 a: &Point<Real>,
101 _: &Point<Real>,
102 nabc: &Vector<Real>,
103 nabd: &Vector<Real>,
104 ap: &Vector<Real>,
105 ab: &Vector<Real>,
106 ap_ab: Real, bp_ab: Real, ) -> (
109 Real,
110 Real,
111 Option<(PointProjection, TetrahedronPointLocation)>,
112 ) {
113 let ab_ab = ap_ab - bp_ab;
114
115 let ap_x_ab = ap.cross(ab);
122 let dabc = ap_x_ab.dot(nabc);
123 let dabd = ap_x_ab.dot(nabd);
124
125 if ab_ab != 0.0 && dabc >= 0.0 && dabd >= 0.0 && ap_ab >= 0.0 && ap_ab <= ab_ab {
127 let u = ap_ab / ab_ab;
129 let bcoords = [1.0 - u, u];
130 let res = a + ab * u;
131 let proj = PointProjection::new(false, res);
132 (
133 dabc,
134 dabd,
135 Some((proj, TetrahedronPointLocation::OnEdge(i as u32, bcoords))),
136 )
137 } else {
138 (dabc, dabd, None)
139 }
140 }
141
142 let nabc = ab.cross(&ac);
146 let nabd = ab.cross(&ad);
147 let (dabc, dabd, res) = check_edge(
148 0, &self.a, &self.b, &nabc, &nabd, &ap, &ab, ap_ab,
149 bp_ab, );
151 if let Some(res) = res {
152 return res;
153 }
154
155 let nacd = ac.cross(&ad);
163 let (dacd, dacb, res) = check_edge(
164 1, &self.a, &self.c, &nacd, &-nabc, &ap, &ac, ap_ac,
165 cp_ac, );
167 if let Some(res) = res {
168 return res;
169 }
170
171 let (dadb, dadc, res) = check_edge(
179 2, &self.a, &self.d, &-nabd, &-nacd, &ap, &ad, ap_ad,
180 dp_ad, );
182 if let Some(res) = res {
183 return res;
184 }
185
186 let nbcd = bc.cross(&bd);
193 let (dbca, dbcd, res) = check_edge(
195 3, &self.b, &self.c, &nabc, &nbcd, &bp, &bc, bp_bc,
196 cp_bc, );
198 if let Some(res) = res {
199 return res;
200 }
201
202 let (dbdc, dbda, res) = check_edge(
212 4, &self.b, &self.d, &-nbcd, &nabd, &bp, &bd, bp_bd,
213 dp_bd, );
215 if let Some(res) = res {
216 return res;
217 }
218
219 let (dcda, dcdb, res) = check_edge(
228 5, &self.c, &self.d, &nacd, &nbcd, &cp, &cd, cp_cd,
229 dp_cd, );
231 if let Some(res) = res {
232 return res;
233 }
234
235 #[inline(always)]
239 fn check_face(
240 i: usize,
241 a: &Point<Real>,
242 b: &Point<Real>,
243 c: &Point<Real>,
244 ap: &Vector<Real>,
245 bp: &Vector<Real>,
246 cp: &Vector<Real>,
247 ab: &Vector<Real>,
248 ac: &Vector<Real>,
249 ad: &Vector<Real>,
250 dabc: Real,
251 dbca: Real,
252 dacb: Real,
253 ) -> Option<(PointProjection, TetrahedronPointLocation)> {
256 if dabc < 0.0 && dbca < 0.0 && dacb < 0.0 {
257 let n = ab.cross(ac); if n.dot(ad) * n.dot(ap) < 0.0 {
259 let normal = n.try_normalize(crate::math::DEFAULT_EPSILON)?;
274 let vc = normal.dot(&ap.cross(bp));
275 let va = normal.dot(&bp.cross(cp));
276 let vb = normal.dot(&cp.cross(ap));
277
278 let denom = va + vb + vc;
279 assert!(denom != 0.0);
280 let inv_denom = 1.0 / denom;
281
282 let bcoords = [va * inv_denom, vb * inv_denom, vc * inv_denom];
283 let res = a * bcoords[0] + b.coords * bcoords[1] + c.coords * bcoords[2];
284 let proj = PointProjection::new(false, res);
285
286 return Some((proj, TetrahedronPointLocation::OnFace(i as u32, bcoords)));
287 }
288 }
289 None
290 }
291
292 if let Some(res) = check_face(
294 0, &self.a, &self.b, &self.c, &ap, &bp, &cp, &ab, &ac, &ad, dabc, dbca,
295 dacb,
296 ) {
299 return res;
300 }
301
302 if let Some(res) = check_face(
304 1, &self.a, &self.b, &self.d, &ap, &bp, &dp, &ab, &ad, &ac, dadb, dabd,
305 dbda,
306 ) {
309 return res;
310 }
311 if let Some(res) = check_face(
313 2, &self.a, &self.c, &self.d, &ap, &cp, &dp, &ac, &ad, &ab, dacd, dcda,
314 dadc,
315 ) {
318 return res;
319 }
320 if let Some(res) = check_face(
322 3, &self.b, &self.c, &self.d, &bp, &cp, &dp, &bc, &bd, &-ab, dbcd, dcdb,
323 dbdc,
324 ) {
327 return res;
328 }
329
330 if !solid {
331 unimplemented!(
333 "Non-solid ray-cast/point projection on a tetrahedron is not yet implemented."
334 )
335 }
336
337 let proj = PointProjection::new(true, *pt);
338 (proj, TetrahedronPointLocation::OnSolid)
339 }
340}