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
|
use crate::geometry::Cylinder;
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 rounded cylinder.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug)]
pub struct RoundCylinder {
/// The cylinder being rounded.
pub cylinder: Cylinder,
/// The rounding radius.
pub border_radius: f32,
}
impl RoundCylinder {
/// Create sa new cylinder 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 cylinder.
pub fn new(half_height: f32, radius: f32, border_radius: f32) -> Self {
Self {
cylinder: Cylinder::new(half_height, radius),
border_radius,
}
}
}
impl SupportMap<f32> for RoundCylinder {
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.cylinder.local_support_point_toward(dir) + **dir * self.border_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 RayCast<f32> for RoundCylinder {
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 PointQuery<f32> for RoundCylinder {
#[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)
}
}
|
