1use crate::math::{Isometry, Point, Real, Rotation, Vector};
2use crate::shape::{Segment, SupportMap};
3use na::Unit;
4
5#[cfg(feature = "alloc")]
6use either::Either;
7
8#[cfg(feature = "rkyv")]
9use rkyv::{bytecheck, CheckBytes};
10
11#[derive(Copy, Clone, Debug)]
12#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
13#[cfg_attr(feature = "bytemuck", derive(bytemuck::Pod, bytemuck::Zeroable))]
14#[cfg_attr(
15 feature = "rkyv",
16 derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes),
17 archive(as = "Self")
18)]
19#[repr(C)]
20pub struct Capsule {
22 pub segment: Segment,
24 pub radius: Real,
26}
27
28impl Capsule {
29 pub fn new_x(half_height: Real, radius: Real) -> Self {
31 let b = Point::from(Vector::x() * half_height);
32 Self::new(-b, b, radius)
33 }
34
35 pub fn new_y(half_height: Real, radius: Real) -> Self {
37 let b = Point::from(Vector::y() * half_height);
38 Self::new(-b, b, radius)
39 }
40
41 #[cfg(feature = "dim3")]
43 pub fn new_z(half_height: Real, radius: Real) -> Self {
44 let b = Point::from(Vector::z() * half_height);
45 Self::new(-b, b, radius)
46 }
47
48 pub fn new(a: Point<Real>, b: Point<Real>, radius: Real) -> Self {
50 let segment = Segment::new(a, b);
51 Self { segment, radius }
52 }
53
54 pub fn height(&self) -> Real {
56 (self.segment.b - self.segment.a).norm()
57 }
58
59 pub fn half_height(&self) -> Real {
61 self.height() / 2.0
62 }
63
64 pub fn center(&self) -> Point<Real> {
66 na::center(&self.segment.a, &self.segment.b)
67 }
68
69 pub fn transform_by(&self, pos: &Isometry<Real>) -> Self {
71 Self::new(pos * self.segment.a, pos * self.segment.b, self.radius)
72 }
73
74 pub fn canonical_transform(&self) -> Isometry<Real> {
77 let tra = self.center().coords;
78 let rot = self.rotation_wrt_y();
79 Isometry::from_parts(tra.into(), rot)
80 }
81
82 pub fn rotation_wrt_y(&self) -> Rotation<Real> {
84 let mut dir = self.segment.b - self.segment.a;
85 if dir.y < 0.0 {
86 dir = -dir;
87 }
88
89 #[cfg(feature = "dim2")]
90 {
91 Rotation::rotation_between(&Vector::y(), &dir)
92 }
93
94 #[cfg(feature = "dim3")]
95 {
96 Rotation::rotation_between(&Vector::y(), &dir).unwrap_or(Rotation::identity())
97 }
98 }
99
100 pub fn transform_wrt_y(&self) -> Isometry<Real> {
102 let rot = self.rotation_wrt_y();
103 Isometry::from_parts(self.center().coords.into(), rot)
104 }
105
106 #[cfg(all(feature = "dim2", feature = "alloc"))]
113 pub fn scaled(
114 self,
115 scale: &Vector<Real>,
116 nsubdivs: u32,
117 ) -> Option<Either<Self, super::ConvexPolygon>> {
118 if scale.x != scale.y {
119 let mut vtx = self.to_polyline(nsubdivs);
121 vtx.iter_mut()
122 .for_each(|pt| pt.coords = pt.coords.component_mul(scale));
123 Some(Either::Right(super::ConvexPolygon::from_convex_polyline(
124 vtx,
125 )?))
126 } else {
127 let uniform_scale = scale.x;
128 Some(Either::Left(Self::new(
129 self.segment.a * uniform_scale,
130 self.segment.b * uniform_scale,
131 self.radius * uniform_scale.abs(),
132 )))
133 }
134 }
135
136 #[cfg(all(feature = "dim3", feature = "alloc"))]
143 pub fn scaled(
144 self,
145 scale: &Vector<Real>,
146 nsubdivs: u32,
147 ) -> Option<Either<Self, super::ConvexPolyhedron>> {
148 if scale.x != scale.y || scale.x != scale.z || scale.y != scale.z {
149 let (mut vtx, idx) = self.to_trimesh(nsubdivs, nsubdivs);
151 vtx.iter_mut()
152 .for_each(|pt| pt.coords = pt.coords.component_mul(scale));
153 Some(Either::Right(super::ConvexPolyhedron::from_convex_mesh(
154 vtx, &idx,
155 )?))
156 } else {
157 let uniform_scale = scale.x;
158 Some(Either::Left(Self::new(
159 self.segment.a * uniform_scale,
160 self.segment.b * uniform_scale,
161 self.radius * uniform_scale.abs(),
162 )))
163 }
164 }
165}
166
167impl SupportMap for Capsule {
168 fn local_support_point(&self, dir: &Vector<Real>) -> Point<Real> {
169 let dir = Unit::try_new(*dir, 0.0).unwrap_or(Vector::y_axis());
170 self.local_support_point_toward(&dir)
171 }
172
173 fn local_support_point_toward(&self, dir: &Unit<Vector<Real>>) -> Point<Real> {
174 if dir.dot(&self.segment.a.coords) > dir.dot(&self.segment.b.coords) {
175 self.segment.a + **dir * self.radius
176 } else {
177 self.segment.b + **dir * self.radius
178 }
179 }
180}