parry3d/shape/
capsule.rs

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)]
20/// A capsule shape defined as a round segment.
21pub struct Capsule {
22    /// The endpoints of the capsule’s principal axis.
23    pub segment: Segment,
24    /// The radius of the capsule.
25    pub radius: Real,
26}
27
28impl Capsule {
29    /// Creates a new capsule aligned with the `x` axis and with the given half-height an radius.
30    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    /// Creates a new capsule aligned with the `y` axis and with the given half-height an radius.
36    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    /// Creates a new capsule aligned with the `z` axis and with the given half-height an radius.
42    #[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    /// Creates a new capsule defined as the segment between `a` and `b` and with the given `radius`.
49    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    /// The height of this capsule.
55    pub fn height(&self) -> Real {
56        (self.segment.b - self.segment.a).norm()
57    }
58
59    /// The half-height of this capsule.
60    pub fn half_height(&self) -> Real {
61        self.height() / 2.0
62    }
63
64    /// The center of this capsule.
65    pub fn center(&self) -> Point<Real> {
66        na::center(&self.segment.a, &self.segment.b)
67    }
68
69    /// Creates a new capsule equal to `self` with all its endpoints transformed by `pos`.
70    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    /// The transformation such that `t * Y` is collinear with `b - a` and `t * origin` equals
75    /// the capsule's center.
76    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    /// The rotation `r` such that `r * Y` is collinear with `b - a`.
83    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    /// The transform `t` such that `t * Y` is collinear with `b - a` and such that `t * origin = (b + a) / 2.0`.
101    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    /// Computes a scaled version of this capsule.
107    ///
108    /// If the scaling factor is non-uniform, then it can’t be represented as
109    /// capsule. Instead, a convex polygon approximation (with `nsubdivs`
110    /// subdivisions) is returned. Returns `None` if that approximation had degenerate
111    /// normals (for example if the scaling factor along one axis is zero).
112    #[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            // The scaled shape is not a capsule.
120            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    /// Computes a scaled version of this capsule.
137    ///
138    /// If the scaling factor is non-uniform, then it can’t be represented as
139    /// capsule. Instead, a convex polygon approximation (with `nsubdivs`
140    /// subdivisions) is returned. Returns `None` if that approximation had degenerate
141    /// normals (for example if the scaling factor along one axis is zero).
142    #[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            // The scaled shape is not a capsule.
150            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}