parry2d/query/point/
point_capsule.rs

1use crate::approx::AbsDiffEq;
2use crate::math::{Real, Vector, VectorExt};
3use crate::query::{PointProjection, PointQuery};
4use crate::shape::{Capsule, FeatureId, Segment};
5
6impl PointQuery for Capsule {
7    #[inline]
8    fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection {
9        let seg = Segment::new(self.segment.a, self.segment.b);
10        let proj = seg.project_local_point(pt, solid);
11        let dproj = pt - proj.point;
12        let (dir, dist) = dproj.normalize_and_length();
13        if dist >= Real::default_epsilon() {
14            let inside = dist <= self.radius;
15            if solid && inside {
16                return PointProjection::new(true, pt);
17            } else {
18                return PointProjection::new(inside, proj.point + dir * self.radius);
19            }
20        } else if solid {
21            return PointProjection::new(true, pt);
22        }
23
24        #[cfg(feature = "dim2")]
25        if let Some(dir) = seg.normal() {
26            PointProjection::new(true, proj.point + dir * self.radius)
27        } else {
28            // The segment has no normal, likely because it degenerates to a point.
29            PointProjection::new(true, proj.point + Vector::ith(1, self.radius))
30        }
31
32        #[cfg(feature = "dim3")]
33        if let Some(dir) = seg.direction() {
34            use crate::utils::WBasis;
35            let dir = dir.orthonormal_basis()[0];
36            PointProjection::new(true, proj.point + dir * self.radius)
37        } else {
38            // The segment has no normal, likely because it degenerates to a point.
39            PointProjection::new(true, proj.point + Vector::ith(1, self.radius))
40        }
41    }
42
43    #[inline]
44    fn project_local_point_and_get_feature(&self, pt: Vector) -> (PointProjection, FeatureId) {
45        (self.project_local_point(pt, false), FeatureId::Face(0))
46    }
47}
parry2d/query/point/point_capsule.rs

parry2d/query/point/
point_capsule.rs
