parry3d_f64/query/point/
point_cylinder.rs

1use crate::math::{Vector, Vector2};
2use crate::query::{PointProjection, PointQuery};
3use crate::shape::{Cylinder, FeatureId};
4
5impl PointQuery for Cylinder {
6    #[inline]
7    fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection {
8        // Project on the basis.
9        let (mut dir_from_basis_center, planar_dist_from_basis_center) =
10            Vector2::new(pt.x, pt.z).normalize_and_length();
11
12        if planar_dist_from_basis_center <= crate::math::DEFAULT_EPSILON {
13            {
14                dir_from_basis_center = Vector2::X;
15            }
16        }
17
18        let proj2d = dir_from_basis_center * self.radius;
19
20        if pt.y >= -self.half_height
21            && pt.y <= self.half_height
22            && planar_dist_from_basis_center <= self.radius
23        {
24            // The point is inside of the cylinder.
25            if solid {
26                PointProjection::new(true, pt)
27            } else {
28                let dist_to_top = self.half_height - pt.y;
29                let dist_to_bottom = pt.y - (-self.half_height);
30                let dist_to_side = self.radius - planar_dist_from_basis_center;
31
32                if dist_to_top < dist_to_bottom && dist_to_top < dist_to_side {
33                    let projection_on_top = Vector::new(pt.x, self.half_height, pt.z);
34                    PointProjection::new(true, projection_on_top)
35                } else if dist_to_bottom < dist_to_top && dist_to_bottom < dist_to_side {
36                    let projection_on_bottom = Vector::new(pt.x, -self.half_height, pt.z);
37                    PointProjection::new(true, projection_on_bottom)
38                } else {
39                    let projection_on_side = Vector::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 = Vector::new(pt.x, self.half_height, pt.z);
48                    PointProjection::new(false, projection_on_top)
49                } else {
50                    let projection_on_top_circle =
51                        Vector::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 = Vector::new(pt.x, -self.half_height, pt.z);
58                    PointProjection::new(false, projection_on_bottom)
59                } else {
60                    let projection_on_bottom_circle =
61                        Vector::new(proj2d[0], -self.half_height, proj2d[1]);
62                    PointProjection::new(false, projection_on_bottom_circle)
63                }
64            } else {
65                // Project on the side.
66                let projection_on_side = Vector::new(proj2d[0], pt.y, proj2d[1]);
67                PointProjection::new(false, projection_on_side)
68            }
69        }
70    }
71
72    #[inline]
73    fn project_local_point_and_get_feature(&self, pt: Vector) -> (PointProjection, FeatureId) {
74        // TODO: get the actual feature.
75        (self.project_local_point(pt, false), FeatureId::Unknown)
76    }
77}
parry3d_f64/query/point/point_cylinder.rs

parry3d_f64/query/point/
point_cylinder.rs
