parry3d_f64/query/point/
point_cone.rs

1use crate::math::{Vector, Vector2};
2use crate::query::{PointProjection, PointQuery};
3use crate::shape::{Cone, FeatureId, Segment};
4
5impl PointQuery for Cone {
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            dir_from_basis_center = Vector2::X;
14        }
15
16        let projection_on_basis = Vector::new(pt.x, -self.half_height, pt.z);
17
18        if pt.y < -self.half_height && planar_dist_from_basis_center <= self.radius {
19            // The projection is on the basis.
20            return PointProjection::new(false, projection_on_basis);
21        }
22
23        // Project on the basis circle.
24        let proj2d = dir_from_basis_center * self.radius;
25        let projection_on_basis_circle = Vector::new(proj2d[0], -self.half_height, proj2d[1]);
26
27        // Project on the conic side.
28        // TODO: we could solve this in 2D using the plane passing through the cone axis and the conic_side_segment to save some computation.
29        let apex_point = Vector::new(0.0, self.half_height, 0.0);
30        let conic_side_segment = Segment::new(apex_point, projection_on_basis_circle);
31        let conic_side_segment_dir = conic_side_segment.scaled_direction();
32        let mut proj = conic_side_segment.project_local_point(pt, true);
33
34        let apex_to_basis_center = Vector::new(0.0, -2.0 * self.half_height, 0.0);
35
36        // Now determine if the point is inside of the cone.
37        if pt.y >= -self.half_height
38            && pt.y <= self.half_height
39            && conic_side_segment_dir
40                .cross(pt - apex_point)
41                .dot(conic_side_segment_dir.cross(apex_to_basis_center))
42                >= 0.0
43        {
44            if solid {
45                PointProjection::new(true, pt)
46            } else {
47                // We are inside of the cone, so the correct projection is
48                // either on the basis of the cone, or on the conic side.
49                if (proj.point - pt).length_squared() > (projection_on_basis - pt).length_squared()
50                {
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(&self, pt: Vector) -> (PointProjection, FeatureId) {
66        // TODO: get the actual feature.
67        (self.project_local_point(pt, false), FeatureId::Unknown)
68    }
69}
parry3d_f64/query/point/point_cone.rs

parry3d_f64/query/point/
point_cone.rs
