parry3d/query/point/
point_segment.rs

1use crate::math::Vector;
2use crate::query::{PointProjection, PointQuery, PointQueryWithLocation};
3use crate::shape::{FeatureId, Segment, SegmentPointLocation};
4use crate::utils::relative_eq_vector;
5
6impl PointQuery for Segment {
7    #[inline]
8    fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection {
9        self.project_local_point_and_get_location(pt, solid).0
10    }
11
12    #[inline]
13    fn project_local_point_and_get_feature(&self, pt: Vector) -> (PointProjection, FeatureId) {
14        let (proj, loc) = self.project_local_point_and_get_location(pt, false);
15        let feature = match loc {
16            SegmentPointLocation::OnVertex(i) => FeatureId::Vertex(i),
17            SegmentPointLocation::OnEdge(..) => {
18                #[cfg(feature = "dim2")]
19                {
20                    let dir = self.scaled_direction();
21                    let dpt = pt - proj.point;
22                    if dpt.perp_dot(dir) >= 0.0 {
23                        FeatureId::Face(0)
24                    } else {
25                        FeatureId::Face(1)
26                    }
27                }
28
29                #[cfg(feature = "dim3")]
30                {
31                    FeatureId::Edge(0)
32                }
33            }
34        };
35
36        (proj, feature)
37    }
38
39    // NOTE: the default implementation of `.distance_to_point(...)` will return the error that was
40    // eaten by the `::approx_eq(...)` on `project_point(...)`.
41}
42
43impl PointQueryWithLocation for Segment {
44    type Location = SegmentPointLocation;
45
46    #[inline]
47    fn project_local_point_and_get_location(
48        &self,
49        pt: Vector,
50        _: bool,
51    ) -> (PointProjection, Self::Location) {
52        let ab = self.b - self.a;
53        let ap = pt - self.a;
54        let ab_ap = ab.dot(ap);
55        let sqnab = ab.length_squared();
56
57        let proj;
58        let location;
59
60        if ab_ap <= 0.0 {
61            // Voronoï region of vertex 'a'.
62            location = SegmentPointLocation::OnVertex(0);
63            proj = self.a;
64        } else if ab_ap >= sqnab {
65            // Voronoï region of vertex 'b'.
66            location = SegmentPointLocation::OnVertex(1);
67            proj = self.b;
68        } else {
69            assert!(sqnab != 0.0);
70
71            // Voronoï region of the segment interior.
72            let u = ab_ap / sqnab;
73            let bcoords = [1.0 - u, u];
74            location = SegmentPointLocation::OnEdge(bcoords);
75            proj = self.a + ab * u;
76        }
77
78        // TODO: is this acceptable?
79        let inside = relative_eq_vector(proj, pt);
80
81        (PointProjection::new(inside, proj), location)
82    }
83}
parry3d/query/point/point_segment.rs

parry3d/query/point/
point_segment.rs
