parry3d/shape/
polygonal_feature_map.rs

1use crate::math::{Real, Vector};
2use crate::shape::{Cuboid, PolygonalFeature, Segment, SupportMap, Triangle};
3use na::Unit;
4#[cfg(feature = "dim3")]
5use {
6    crate::{
7        math::Point,
8        shape::{Cone, Cylinder, PackedFeatureId},
9    },
10    approx::AbsDiffEq,
11};
12
13#[cfg(not(feature = "alloc"))]
14use na::{ComplexField, RealField}; // for .abs() and .copysign()
15
16/// Trait implemented by convex shapes with features with polyhedral approximations.
17pub trait PolygonalFeatureMap: SupportMap {
18    /// Compute the support polygonal face of `self` towards the `dir`.
19    fn local_support_feature(&self, dir: &Unit<Vector<Real>>, out_feature: &mut PolygonalFeature);
20
21    // TODO: this is currently just a workaround for https://github.com/dimforge/rapier/issues/417
22    //       until we get a better way to deal with the issue without breaking internal edges
23    //       handling.
24    /// Is this shape a `ConvexPolyhedron`?
25    fn is_convex_polyhedron(&self) -> bool {
26        false
27    }
28}
29
30impl PolygonalFeatureMap for Segment {
31    fn local_support_feature(&self, _: &Unit<Vector<Real>>, out_feature: &mut PolygonalFeature) {
32        *out_feature = PolygonalFeature::from(*self);
33    }
34}
35
36impl PolygonalFeatureMap for Triangle {
37    fn local_support_feature(&self, dir: &Unit<Vector<Real>>, out_feature: &mut PolygonalFeature) {
38        *out_feature = self.support_face(**dir);
39    }
40}
41
42impl PolygonalFeatureMap for Cuboid {
43    fn local_support_feature(&self, dir: &Unit<Vector<Real>>, out_feature: &mut PolygonalFeature) {
44        *out_feature = self.support_face(**dir);
45    }
46}
47
48#[cfg(feature = "dim3")]
49impl PolygonalFeatureMap for Cylinder {
50    fn local_support_feature(&self, dir: &Unit<Vector<Real>>, out_features: &mut PolygonalFeature) {
51        use na::Vector2;
52
53        // About feature ids.
54        // At all times, we consider our cylinder to be approximated as follows:
55        // - The curved part is approximated by a single segment.
56        // - Each flat cap of the cylinder is approximated by a square.
57        // - The curved-part segment has a feature ID of 0, and its endpoint with negative
58        //   `y` coordinate has an ID of 1.
59        // - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order
60        //   when looking at the cap with an eye looking towards +y).
61        // - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order.
62        // - The bottom cap has its face feature ID of 9.
63        // - The feature IDs of the top cap are the same as the bottom cap to which we add 10.
64        //   So its vertices have IDs 11,13,15,17, its edges 12,14,16,18, and its face 19.
65        // - Note that at all times, one of each cap's vertices are the same as the curved-part
66        //   segment endpoints.
67        let dir2 = Vector2::new(dir.x, dir.z)
68            .try_normalize(Real::default_epsilon())
69            .unwrap_or(Vector2::x());
70
71        if dir.y.abs() < 0.5 {
72            // We return a segment lying on the cylinder's curved part.
73            out_features.vertices[0] = Point::new(
74                dir2.x * self.radius,
75                -self.half_height,
76                dir2.y * self.radius,
77            );
78            out_features.vertices[1] =
79                Point::new(dir2.x * self.radius, self.half_height, dir2.y * self.radius);
80            out_features.eids = PackedFeatureId::edges([0, 0, 0, 0]);
81            out_features.fid = PackedFeatureId::face(0);
82            out_features.num_vertices = 2;
83            out_features.vids = PackedFeatureId::vertices([1, 11, 11, 11]);
84        } else {
85            // We return a square approximation of the cylinder cap.
86            let y = self.half_height.copysign(dir.y);
87            out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius);
88            out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius);
89            out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius);
90            out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius);
91
92            if dir.y < 0.0 {
93                out_features.eids = PackedFeatureId::edges([2, 4, 6, 8]);
94                out_features.fid = PackedFeatureId::face(9);
95                out_features.num_vertices = 4;
96                out_features.vids = PackedFeatureId::vertices([1, 3, 5, 7]);
97            } else {
98                out_features.eids = PackedFeatureId::edges([12, 14, 16, 18]);
99                out_features.fid = PackedFeatureId::face(19);
100                out_features.num_vertices = 4;
101                out_features.vids = PackedFeatureId::vertices([11, 13, 15, 17]);
102            }
103        }
104    }
105}
106
107#[cfg(feature = "dim3")]
108impl PolygonalFeatureMap for Cone {
109    fn local_support_feature(&self, dir: &Unit<Vector<Real>>, out_features: &mut PolygonalFeature) {
110        use na::Vector2;
111
112        // About feature ids. It is very similar to the feature ids of cylinders.
113        // At all times, we consider our cone to be approximated as follows:
114        // - The curved part is approximated by a single segment.
115        // - The flat cap of the cone is approximated by a square.
116        // - The curved-part segment has a feature ID of 0, and its endpoint with negative
117        //   `y` coordinate has an ID of 1.
118        // - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order
119        //   when looking at the cap with an eye looking towards +y).
120        // - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order.
121        // - The bottom cap has its face feature ID of 9.
122        // - Note that at all times, one of the cap's vertices are the same as the curved-part
123        //   segment endpoints.
124        let dir2 = Vector2::new(dir.x, dir.z)
125            .try_normalize(Real::default_epsilon())
126            .unwrap_or(Vector2::x());
127
128        if dir.y > 0.0 {
129            // We return a segment lying on the cone's curved part.
130            out_features.vertices[0] = Point::new(
131                dir2.x * self.radius,
132                -self.half_height,
133                dir2.y * self.radius,
134            );
135            out_features.vertices[1] = Point::new(0.0, self.half_height, 0.0);
136            out_features.eids = PackedFeatureId::edges([0, 0, 0, 0]);
137            out_features.fid = PackedFeatureId::face(0);
138            out_features.num_vertices = 2;
139            out_features.vids = PackedFeatureId::vertices([1, 11, 11, 11]);
140        } else {
141            // We return a square approximation of the cone cap.
142            let y = -self.half_height;
143            out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius);
144            out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius);
145            out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius);
146            out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius);
147
148            out_features.eids = PackedFeatureId::edges([2, 4, 6, 8]);
149            out_features.fid = PackedFeatureId::face(9);
150            out_features.num_vertices = 4;
151            out_features.vids = PackedFeatureId::vertices([1, 3, 5, 7]);
152        }
153    }
154}
parry3d/shape/polygonal_feature_map.rs

parry3d/shape/
polygonal_feature_map.rs
