parry3d/query/point/
point_cone.rs

1use crate::math::{Point, Real, Vector};
2use crate::query::{PointProjection, PointQuery};
3use crate::shape::{Cone, FeatureId, Segment};
4use na;
5
6impl PointQuery for Cone {
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 projection_on_basis = Point::new(pt.coords.x, -self.half_height, pt.coords.z);
18
19        if pt.y < -self.half_height && planar_dist_from_basis_center <= self.radius {
20            // The projection is on the basis.
21            return PointProjection::new(false, projection_on_basis);
22        }
23
24        // Project on the basis circle.
25        let proj2d = dir_from_basis_center * self.radius;
26        let projection_on_basis_circle = Point::new(proj2d[0], -self.half_height, proj2d[1]);
27
28        // Project on the conic side.
29        // TODO: we could solve this in 2D using the plane passing through the cone axis and the conic_side_segment to save some computation.
30        let apex_point = Point::new(0.0, self.half_height, 0.0);
31        let conic_side_segment = Segment::new(apex_point, projection_on_basis_circle);
32        let conic_side_segment_dir = conic_side_segment.scaled_direction();
33        let mut proj = conic_side_segment.project_local_point(pt, true);
34
35        let apex_to_basis_center = Vector::new(0.0, -2.0 * self.half_height, 0.0);
36
37        // Now determine if the point is inside of the cone.
38        if pt.y >= -self.half_height
39            && pt.y <= self.half_height
40            && conic_side_segment_dir
41                .cross(&(pt - apex_point))
42                .dot(&conic_side_segment_dir.cross(&apex_to_basis_center))
43                >= 0.0
44        {
45            if solid {
46                PointProjection::new(true, *pt)
47            } else {
48                // We are inside of the cone, so the correct projection is
49                // either on the basis of the cone, or on the conic side.
50                if (proj.point - pt).norm_squared() > (projection_on_basis - pt).norm_squared() {
51                    PointProjection::new(true, projection_on_basis)
52                } else {
53                    proj.is_inside = true;
54                    proj
55                }
56            }
57        } else {
58            // We are outside of the cone, return the computed proj
59            // as-is.
60            proj
61        }
62    }
63
64    #[inline]
65    fn project_local_point_and_get_feature(
66        &self,
67        pt: &Point<Real>,
68    ) -> (PointProjection, FeatureId) {
69        // TODO: get the actual feature.
70        (self.project_local_point(pt, false), FeatureId::Unknown)
71    }
72}
parry3d/query/point/point_cone.rs

parry3d/query/point/
point_cone.rs
