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 144 145 146 147 148 149 150 151 152 153 154
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, PackedFeatureId},
},
approx::AbsDiffEq,
};
#[cfg(not(feature = "std"))]
use na::{ComplexField, RealField}; // for .abs() and .copysign()
/// 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);
// TODO: this is currently just a workaround for https://github.com/dimforge/rapier/issues/417
// until we get a better way to deal with the issue without breaking internal edges
// handling.
/// Is this shape a `ConvexPolyhedron`?
fn is_convex_polyhedron(&self) -> bool {
false
}
}
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);
}
}
#[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 = PackedFeatureId::edges([0, 0, 0, 0]);
out_features.fid = PackedFeatureId::face(0);
out_features.num_vertices = 2;
out_features.vids = PackedFeatureId::vertices([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 = PackedFeatureId::edges([2, 4, 6, 8]);
out_features.fid = PackedFeatureId::face(9);
out_features.num_vertices = 4;
out_features.vids = PackedFeatureId::vertices([1, 3, 5, 7]);
} else {
out_features.eids = PackedFeatureId::edges([12, 14, 16, 18]);
out_features.fid = PackedFeatureId::face(19);
out_features.num_vertices = 4;
out_features.vids = PackedFeatureId::vertices([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 = PackedFeatureId::edges([0, 0, 0, 0]);
out_features.fid = PackedFeatureId::face(0);
out_features.num_vertices = 2;
out_features.vids = PackedFeatureId::vertices([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 = PackedFeatureId::edges([2, 4, 6, 8]);
out_features.fid = PackedFeatureId::face(9);
out_features.num_vertices = 4;
out_features.vids = PackedFeatureId::vertices([1, 3, 5, 7]);
}
}
}