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
|
#[cfg(feature = "dim3")]
use crate::geometry::Cylinder;
use crate::geometry::ShapeType;
use crate::math::{Isometry, Point, Vector};
use na::Unit;
use ncollide::query::{
algorithms::VoronoiSimplex, PointProjection, PointQuery, Ray, RayCast, RayIntersection,
};
use ncollide::shape::{FeatureId, SupportMap};
/// A shape which corner can be rounded.
pub trait Roundable {
/// The ShapeType fo this shape after rounding its corners.
fn rounded_shape_type() -> ShapeType;
}
#[cfg(feature = "dim3")]
impl Roundable for Cylinder {
fn rounded_shape_type() -> ShapeType {
ShapeType::RoundCylinder
}
}
/// A rounded shape.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct Rounded<S: Roundable> {
/// The shape being rounded.
pub shape: S,
/// The rounding radius.
pub radius: f32,
}
impl<S: Roundable> Rounded<S> {
/// Create sa new shape where all its edges and vertices are rounded by a radius of `radius`.
///
/// This is done by applying a dilation of the given radius to the shape.
pub fn new(shape: S, radius: f32) -> Self {
Self { shape, radius }
}
}
impl<S: Roundable + SupportMap<f32>> SupportMap<f32> for Rounded<S> {
fn local_support_point(&self, dir: &Vector<f32>) -> Point<f32> {
self.local_support_point_toward(&Unit::new_normalize(*dir))
}
fn local_support_point_toward(&self, dir: &Unit<Vector<f32>>) -> Point<f32> {
self.shape.local_support_point_toward(dir) + **dir * self.radius
}
fn support_point(&self, transform: &Isometry<f32>, dir: &Vector<f32>) -> Point<f32> {
let local_dir = transform.inverse_transform_vector(dir);
transform * self.local_support_point(&local_dir)
}
fn support_point_toward(
&self,
transform: &Isometry<f32>,
dir: &Unit<Vector<f32>>,
) -> Point<f32> {
let local_dir = Unit::new_unchecked(transform.inverse_transform_vector(dir));
transform * self.local_support_point_toward(&local_dir)
}
}
impl<S: Roundable + SupportMap<f32>> RayCast<f32> for Rounded<S> {
fn toi_and_normal_with_ray(
&self,
m: &Isometry<f32>,
ray: &Ray<f32>,
max_toi: f32,
solid: bool,
) -> Option<RayIntersection<f32>> {
let ls_ray = ray.inverse_transform_by(m);
ncollide::query::ray_intersection_with_support_map_with_params(
&Isometry::identity(),
self,
&mut VoronoiSimplex::new(),
&ls_ray,
max_toi,
solid,
)
.map(|mut res| {
res.normal = m * res.normal;
res
})
}
}
// TODO: if PointQuery had a `project_point_with_normal` method, we could just
// call this and adjust the projected point accordingly.
impl<S: Roundable + SupportMap<f32>> PointQuery<f32> for Rounded<S> {
#[inline]
fn project_point(
&self,
m: &Isometry<f32>,
point: &Point<f32>,
solid: bool,
) -> PointProjection<f32> {
ncollide::query::point_projection_on_support_map(
m,
self,
&mut VoronoiSimplex::new(),
point,
solid,
)
}
#[inline]
fn project_point_with_feature(
&self,
m: &Isometry<f32>,
point: &Point<f32>,
) -> (PointProjection<f32>, FeatureId) {
(self.project_point(m, point, false), FeatureId::Unknown)
}
}
|
