ncollide3d/query/point/
point_tetrahedron.rs

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        /*
49         * Voronoï regions of vertices.
50         */
51        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            // Voronoï region of `a`.
59            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            // Voronoï region of `b`.
73            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            // Voronoï region of `c`.
86            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            // Voronoï region of `d`.
98            let proj = PointProjection::new(false, m * self.d);
99            return (proj, TetrahedronPointLocation::OnVertex(3));
100        }
101
102        /*
103         * Voronoï regions of edges.
104         */
105        #[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, /*ap_ac: N, ap_ad: N,*/
116            bp_ab: N, /*bp_ac: N, bp_ad: N*/
117        ) -> (
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            // NOTE: The following avoids the subsequent cross and dot products but are not
128            // numerically stable.
129            //
130            // let dabc  = ap_ab * (ap_ac - bp_ac) - ap_ac * ab_ab;
131            // let dabd  = ap_ab * (ap_ad - bp_ad) - ap_ad * ab_ab;
132
133            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            // FIXME: the case where ab_ab == _0 is not well defined.
138            if ab_ab != _0 && dabc >= _0 && dabd >= _0 && ap_ab >= _0 && ap_ab <= ab_ab {
139                // Voronoi region of `ab`.
140                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        // Voronoï region of ab.
155        //            let bp_ad = bp_bd + bp_ab;
156        //            let bp_ac = bp_bc + bp_ab;
157        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            /*ap_ac, ap_ad,*/ bp_ab, /*, bp_ac, bp_ad*/
162        );
163        if let Some(res) = res {
164            return res;
165        }
166
167        // Voronoï region of ac.
168        // Substitutions (wrt. ab):
169        //   b -> c
170        //   c -> d
171        //   d -> b
172        //            let cp_ab = cp_ac - cp_bc;
173        //            let cp_ad = cp_cd + cp_ac;
174        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            /*ap_ad, ap_ab,*/ cp_ac, /*, cp_ad, cp_ab*/
178        );
179        if let Some(res) = res {
180            return res;
181        }
182
183        // Voronoï region of ad.
184        // Substitutions (wrt. ab):
185        //   b -> d
186        //   c -> b
187        //   d -> c
188        //            let dp_ac = dp_ad - dp_cd;
189        //            let dp_ab = dp_ad - dp_bd;
190        let (dadb, dadc, res) = check_edge(
191            2, m, &self.a, &self.d, &-nabd, &-nacd, &ap, &ad, ap_ad,
192            /*ap_ab, ap_ac,*/ dp_ad, /*, dp_ab, dp_ac*/
193        );
194        if let Some(res) = res {
195            return res;
196        }
197
198        // Voronoï region of bc.
199        // Substitutions (wrt. ab):
200        //   a -> b
201        //   b -> c
202        //   c -> a
203        //            let cp_bd = cp_cd + cp_bc;
204        let nbcd = bc.cross(&bd);
205        // NOTE: nabc = nbcd
206        let (dbca, dbcd, res) = check_edge(
207            3, m, &self.b, &self.c, &nabc, &nbcd, &bp, &bc, bp_bc,
208            /*-bp_ab, bp_bd,*/ cp_bc, /*, -cp_ab, cp_bd*/
209        );
210        if let Some(res) = res {
211            return res;
212        }
213
214        // Voronoï region of bd.
215        // Substitutions (wrt. ab):
216        //   a -> b
217        //   b -> d
218        //   d -> a
219
220        //            let dp_bc = dp_bd - dp_cd;
221        // NOTE: nbdc = -nbcd
222        // NOTE: nbda = nabd
223        let (dbdc, dbda, res) = check_edge(
224            4, m, &self.b, &self.d, &-nbcd, &nabd, &bp, &bd, bp_bd,
225            /*bp_bc, -bp_ab,*/ dp_bd, /*, dp_bc, -dp_ab*/
226        );
227        if let Some(res) = res {
228            return res;
229        }
230
231        // Voronoï region of cd.
232        // Substitutions (wrt. ab):
233        //   a -> c
234        //   b -> d
235        //   c -> a
236        //   d -> b
237        // NOTE: ncda = nacd
238        // NOTE: ncdb = nbcd
239        let (dcda, dcdb, res) = check_edge(
240            5, m, &self.c, &self.d, &nacd, &nbcd, &cp, &cd, cp_cd,
241            /*-cp_ac, -cp_bc,*/ dp_cd, /*, -dp_ac, -dp_bc*/
242        );
243        if let Some(res) = res {
244            return res;
245        }
246
247        /*
248         * Voronoï regions of faces.
249         */
250        #[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            /* ap_ab: N, bp_ab: N, cp_ab: N,
267            ap_ac: N, bp_ac: N, cp_ac: N, */
268        ) -> 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); // FIXME: is is possible to avoid this cross product?
274                if n.dot(ad) * n.dot(ap) < _0 {
275                    // Voronoï region of the face.
276
277                    // NOTE:
278                    // The following avoids expansive computations but are not very
279                    // numerically stable.
280                    //
281                    // let va = bp_ab * cp_ac - cp_ab * bp_ac;
282                    // let vb = cp_ab * ap_ac - ap_ab * cp_ac;
283                    // let vc = ap_ab * bp_ac - bp_ab * ap_ac;
284
285                    // NOTE: the normalization may fail even if the dot products
286                    // above were < 0. This happens, e.g., when we use fixed-point
287                    // numbers and there are not enough decimal bits to perform
288                    // the normalization.
289                    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        // Face abc.
309        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            /*ap_ab, bp_ab, cp_ab,
313            ap_ac, bp_ac, cp_ac*/
314        ) {
315            return res;
316        }
317
318        // Face abd.
319        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            /*ap_ab, bp_ab, dp_ab,
323            ap_ad, bp_ad, dp_ad*/
324        ) {
325            return res;
326        }
327        // Face acd.
328        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            /*ap_ac, cp_ac, dp_ac,
332            ap_ad, cp_ad, dp_ad*/
333        ) {
334            return res;
335        }
336        // Face bcd.
337        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            /*bp_bc, cp_bc, dp_bc,
341            bp_bd, cp_bd, dp_bd*/
342        ) {
343            return res;
344        }
345
346        if !solid {
347            // XXX: implement the non-solid projection.
348            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}