parry3d/query/point/
point_cylinder.rs

1use crate::math::{Point, Real};
2use crate::query::{PointProjection, PointQuery};
3use crate::shape::{Cylinder, FeatureId};
4use na;
5
6impl PointQuery for Cylinder {
7    #[inline]
8    fn project_local_point(&self, pt: &Point<Real>, solid: bool) -> PointProjection {
9        // Project on the basis.
10        let mut dir_from_basis_center = pt.coords.xz();
11        let planar_dist_from_basis_center = dir_from_basis_center.normalize_mut();
12
13        if planar_dist_from_basis_center <= crate::math::DEFAULT_EPSILON {
14            dir_from_basis_center = na::Vector2::x();
15        }
16
17        let proj2d = dir_from_basis_center * self.radius;
18
19        if pt.y >= -self.half_height
20            && pt.y <= self.half_height
21            && planar_dist_from_basis_center <= self.radius
22        {
23            // The point is inside of the cylinder.
24            if solid {
25                PointProjection::new(true, *pt)
26            } else {
27                let dist_to_top = self.half_height - pt.coords.y;
28                let dist_to_bottom = pt.coords.y - (-self.half_height);
29                let dist_to_side = self.radius - planar_dist_from_basis_center;
30
31                if dist_to_top < dist_to_bottom && dist_to_top < dist_to_side {
32                    let projection_on_top = Point::new(pt.coords.x, self.half_height, pt.coords.z);
33                    PointProjection::new(true, projection_on_top)
34                } else if dist_to_bottom < dist_to_top && dist_to_bottom < dist_to_side {
35                    let projection_on_bottom =
36                        Point::new(pt.coords.x, -self.half_height, pt.coords.z);
37                    PointProjection::new(true, projection_on_bottom)
38                } else {
39                    let projection_on_side = Point::new(proj2d[0], pt.y, proj2d[1]);
40                    PointProjection::new(true, projection_on_side)
41                }
42            }
43        } else {
44            // The point is outside of the cylinder.
45            if pt.y > self.half_height {
46                if planar_dist_from_basis_center <= self.radius {
47                    let projection_on_top = Point::new(pt.coords.x, self.half_height, pt.coords.z);
48                    PointProjection::new(false, projection_on_top)
49                } else {
50                    let projection_on_top_circle =
51                        Point::new(proj2d[0], self.half_height, proj2d[1]);
52                    PointProjection::new(false, projection_on_top_circle)
53                }
54            } else if pt.y < -self.half_height {
55                // Project on the bottom plane or the bottom circle.
56                if planar_dist_from_basis_center <= self.radius {
57                    let projection_on_bottom =
58                        Point::new(pt.coords.x, -self.half_height, pt.coords.z);
59                    return PointProjection::new(false, projection_on_bottom);
60                } else {
61                    let projection_on_bottom_circle =
62                        Point::new(proj2d[0], -self.half_height, proj2d[1]);
63                    return PointProjection::new(false, projection_on_bottom_circle);
64                }
65            } else {
66                // Project on the side.
67                let projection_on_side = Point::new(proj2d[0], pt.y, proj2d[1]);
68                return PointProjection::new(false, projection_on_side);
69            }
70        }
71    }
72
73    #[inline]
74    fn project_local_point_and_get_feature(
75        &self,
76        pt: &Point<Real>,
77    ) -> (PointProjection, FeatureId) {
78        // TODO: get the actual feature.
79        (self.project_local_point(pt, false), FeatureId::Unknown)
80    }
81}
parry3d/query/point/point_cylinder.rs

parry3d/query/point/
point_cylinder.rs
