parry3d/query/point/
point_tetrahedron.rs

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        /*
43         * Voronoï regions of vertices.
44         */
45        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            // Voronoï region of `a`.
51            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            // Voronoï region of `b`.
65            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            // Voronoï region of `c`.
78            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            // Voronoï region of `d`.
90            let proj = PointProjection::new(false, self.d);
91            return (proj, TetrahedronPointLocation::OnVertex(3));
92        }
93
94        /*
95         * Voronoï regions of edges.
96         */
97        #[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, /*ap_ac: Real, ap_ad: Real,*/
107            bp_ab: Real, /*bp_ac: Real, bp_ad: Real*/
108        ) -> (
109            Real,
110            Real,
111            Option<(PointProjection, TetrahedronPointLocation)>,
112        ) {
113            let ab_ab = ap_ab - bp_ab;
114
115            // NOTE: The following avoids the subsequent cross and dot products but are not
116            // numerically stable.
117            //
118            // let dabc  = ap_ab * (ap_ac - bp_ac) - ap_ac * ab_ab;
119            // let dabd  = ap_ab * (ap_ad - bp_ad) - ap_ad * ab_ab;
120
121            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            // TODO: the case where ab_ab == 0.0 is not well defined.
126            if ab_ab != 0.0 && dabc >= 0.0 && dabd >= 0.0 && ap_ab >= 0.0 && ap_ab <= ab_ab {
127                // Voronoi region of `ab`.
128                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        // Voronoï region of ab.
143        //            let bp_ad = bp_bd + bp_ab;
144        //            let bp_ac = bp_bc + bp_ab;
145        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            /*ap_ac, ap_ad,*/ bp_ab, /*, bp_ac, bp_ad*/
150        );
151        if let Some(res) = res {
152            return res;
153        }
154
155        // Voronoï region of ac.
156        // Substitutions (wrt. ab):
157        //   b -> c
158        //   c -> d
159        //   d -> b
160        //            let cp_ab = cp_ac - cp_bc;
161        //            let cp_ad = cp_cd + cp_ac;
162        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            /*ap_ad, ap_ab,*/ cp_ac, /*, cp_ad, cp_ab*/
166        );
167        if let Some(res) = res {
168            return res;
169        }
170
171        // Voronoï region of ad.
172        // Substitutions (wrt. ab):
173        //   b -> d
174        //   c -> b
175        //   d -> c
176        //            let dp_ac = dp_ad - dp_cd;
177        //            let dp_ab = dp_ad - dp_bd;
178        let (dadb, dadc, res) = check_edge(
179            2, &self.a, &self.d, &-nabd, &-nacd, &ap, &ad, ap_ad,
180            /*ap_ab, ap_ac,*/ dp_ad, /*, dp_ab, dp_ac*/
181        );
182        if let Some(res) = res {
183            return res;
184        }
185
186        // Voronoï region of bc.
187        // Substitutions (wrt. ab):
188        //   a -> b
189        //   b -> c
190        //   c -> a
191        //            let cp_bd = cp_cd + cp_bc;
192        let nbcd = bc.cross(&bd);
193        // NOTE: nabc = nbcd
194        let (dbca, dbcd, res) = check_edge(
195            3, &self.b, &self.c, &nabc, &nbcd, &bp, &bc, bp_bc,
196            /*-bp_ab, bp_bd,*/ cp_bc, /*, -cp_ab, cp_bd*/
197        );
198        if let Some(res) = res {
199            return res;
200        }
201
202        // Voronoï region of bd.
203        // Substitutions (wrt. ab):
204        //   a -> b
205        //   b -> d
206        //   d -> a
207
208        //            let dp_bc = dp_bd - dp_cd;
209        // NOTE: nbdc = -nbcd
210        // NOTE: nbda = nabd
211        let (dbdc, dbda, res) = check_edge(
212            4, &self.b, &self.d, &-nbcd, &nabd, &bp, &bd, bp_bd,
213            /*bp_bc, -bp_ab,*/ dp_bd, /*, dp_bc, -dp_ab*/
214        );
215        if let Some(res) = res {
216            return res;
217        }
218
219        // Voronoï region of cd.
220        // Substitutions (wrt. ab):
221        //   a -> c
222        //   b -> d
223        //   c -> a
224        //   d -> b
225        // NOTE: ncda = nacd
226        // NOTE: ncdb = nbcd
227        let (dcda, dcdb, res) = check_edge(
228            5, &self.c, &self.d, &nacd, &nbcd, &cp, &cd, cp_cd,
229            /*-cp_ac, -cp_bc,*/ dp_cd, /*, -dp_ac, -dp_bc*/
230        );
231        if let Some(res) = res {
232            return res;
233        }
234
235        /*
236         * Voronoï regions of faces.
237         */
238        #[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            /* ap_ab: Real, bp_ab: Real, cp_ab: Real,
254            ap_ac: Real, bp_ac: Real, cp_ac: Real, */
255        ) -> Option<(PointProjection, TetrahedronPointLocation)> {
256            if dabc < 0.0 && dbca < 0.0 && dacb < 0.0 {
257                let n = ab.cross(ac); // TODO: is is possible to avoid this cross product?
258                if n.dot(ad) * n.dot(ap) < 0.0 {
259                    // Voronoï region of the face.
260
261                    // NOTE:
262                    // The following avoids expansive computations but are not very
263                    // numerically stable.
264                    //
265                    // let va = bp_ab * cp_ac - cp_ab * bp_ac;
266                    // let vb = cp_ab * ap_ac - ap_ab * cp_ac;
267                    // let vc = ap_ab * bp_ac - bp_ab * ap_ac;
268
269                    // NOTE: the normalization may fail even if the dot products
270                    // above were < 0. This happens, e.g., when we use fixed-point
271                    // numbers and there are not enough decimal bits to perform
272                    // the normalization.
273                    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        // Face abc.
293        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            /*ap_ab, bp_ab, cp_ab,
297            ap_ac, bp_ac, cp_ac*/
298        ) {
299            return res;
300        }
301
302        // Face abd.
303        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            /*ap_ab, bp_ab, dp_ab,
307            ap_ad, bp_ad, dp_ad*/
308        ) {
309            return res;
310        }
311        // Face acd.
312        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            /*ap_ac, cp_ac, dp_ac,
316            ap_ad, cp_ad, dp_ad*/
317        ) {
318            return res;
319        }
320        // Face bcd.
321        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            /*bp_bc, cp_bc, dp_bc,
325            bp_bd, cp_bd, dp_bd*/
326        ) {
327            return res;
328        }
329
330        if !solid {
331            // XXX: implement the non-solid projection.
332            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}