parry3d/query/point/
point_capsule.rs

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

parry3d/query/point/
point_capsule.rs
