1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143

use crate::math::{Real, Vector};
use crate::shape::{Cuboid, PolygonalFeature, Segment, SupportMap, Triangle};
use na::Unit;
#[cfg(feature = "dim3")]
use {
    crate::{
        math::Point,
        shape::{Cone, Cylinder},
    },
    approx::AbsDiffEq,
};

/// Trait implemented by convex shapes with features with polyhedral approximations.
pub trait PolygonalFeatureMap: SupportMap {
    /// Compute the support polygonal face of `self` towards the `dir`.
    fn local_support_feature(&self, dir: &Unit<Vector<Real>>, out_feature: &mut PolygonalFeature);
}

impl PolygonalFeatureMap for Segment {
    fn local_support_feature(&self, _: &Unit<Vector<Real>>, out_feature: &mut PolygonalFeature) {
        *out_feature = PolygonalFeature::from(*self);
    }
}

impl PolygonalFeatureMap for Triangle {
    fn local_support_feature(&self, dir: &Unit<Vector<Real>>, out_feature: &mut PolygonalFeature) {
        *out_feature = self.support_face(**dir);
    }
}

impl PolygonalFeatureMap for Cuboid {
    fn local_support_feature(&self, dir: &Unit<Vector<Real>>, out_feature: &mut PolygonalFeature) {
        *out_feature = self.support_face(**dir).into();
    }
}

#[cfg(feature = "dim3")]
impl PolygonalFeatureMap for Cylinder {
    fn local_support_feature(&self, dir: &Unit<Vector<Real>>, out_features: &mut PolygonalFeature) {
        use na::Vector2;

        // About feature ids.
        // At all times, we consider our cylinder to be approximated as follows:
        // - The curved part is approximated by a single segment.
        // - Each flat cap of the cylinder is approximated by a square.
        // - The curved-part segment has a feature ID of 0, and its endpoint with negative
        //   `y` coordinate has an ID of 1.
        // - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order
        //   when looking at the cap with an eye looking towards +y).
        // - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order.
        // - The bottom cap has its face feature ID of 9.
        // - The feature IDs of the top cap are the same as the bottom cap to which we add 10.
        //   So its vertices have IDs 11,13,15,17, its edges 12,14,16,18, and its face 19.
        // - Note that at all times, one of each cap's vertices are the same as the curved-part
        //   segment endpoints.
        let dir2 = Vector2::new(dir.x, dir.z)
            .try_normalize(Real::default_epsilon())
            .unwrap_or(Vector2::x());

        if dir.y.abs() < 0.5 {
            // We return a segment lying on the cylinder's curved part.
            out_features.vertices[0] = Point::new(
                dir2.x * self.radius,
                -self.half_height,
                dir2.y * self.radius,
            );
            out_features.vertices[1] =
                Point::new(dir2.x * self.radius, self.half_height, dir2.y * self.radius);
            out_features.eids = [0, 0, 0, 0];
            out_features.fid = 0;
            out_features.num_vertices = 2;
            out_features.vids = [1, 11, 11, 11];
        } else {
            // We return a square approximation of the cylinder cap.
            let y = self.half_height.copysign(dir.y);
            out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius);
            out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius);
            out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius);
            out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius);

            if dir.y < 0.0 {
                out_features.eids = [2, 4, 6, 8];
                out_features.fid = 9;
                out_features.num_vertices = 4;
                out_features.vids = [1, 3, 5, 7];
            } else {
                out_features.eids = [12, 14, 16, 18];
                out_features.fid = 19;
                out_features.num_vertices = 4;
                out_features.vids = [11, 13, 15, 17];
            }
        }
    }
}

#[cfg(feature = "dim3")]
impl PolygonalFeatureMap for Cone {
    fn local_support_feature(&self, dir: &Unit<Vector<Real>>, out_features: &mut PolygonalFeature) {
        use na::Vector2;

        // About feature ids. It is very similar to the feature ids of cylinders.
        // At all times, we consider our cone to be approximated as follows:
        // - The curved part is approximated by a single segment.
        // - The flat cap of the cone is approximated by a square.
        // - The curved-part segment has a feature ID of 0, and its endpoint with negative
        //   `y` coordinate has an ID of 1.
        // - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order
        //   when looking at the cap with an eye looking towards +y).
        // - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order.
        // - The bottom cap has its face feature ID of 9.
        // - Note that at all times, one of the cap's vertices are the same as the curved-part
        //   segment endpoints.
        let dir2 = Vector2::new(dir.x, dir.z)
            .try_normalize(Real::default_epsilon())
            .unwrap_or(Vector2::x());

        if dir.y > 0.0 {
            // We return a segment lying on the cone's curved part.
            out_features.vertices[0] = Point::new(
                dir2.x * self.radius,
                -self.half_height,
                dir2.y * self.radius,
            );
            out_features.vertices[1] = Point::new(0.0, self.half_height, 0.0);
            out_features.eids = [0, 0, 0, 0];
            out_features.fid = 0;
            out_features.num_vertices = 2;
            out_features.vids = [1, 11, 11, 11];
        } else {
            // We return a square approximation of the cone cap.
            let y = -self.half_height;
            out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius);
            out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius);
            out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius);
            out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius);

            out_features.eids = [2, 4, 6, 8];
            out_features.fid = 9;
            out_features.num_vertices = 4;
            out_features.vids = [1, 3, 5, 7];
        }
    }
}