parry3d/query/sweep_toi/
toi_proxy.rs

1use crate::bounding_volume::Aabb;
2use crate::math::{Pose, Real, Vector};
3use crate::shape::{Shape, TypedShape};
4
5/// Maximum number of proxy points stored inline (cuboid corners).
6#[cfg(feature = "dim2")]
7pub const TOI_PROXY_INLINE_POINTS: usize = 4;
8/// Maximum number of proxy points stored inline (cuboid corners).
9#[cfg(feature = "dim3")]
10pub const TOI_PROXY_INLINE_POINTS: usize = 8;
11
12enum ProxyPoints<'a> {
13    Inline([Vector; TOI_PROXY_INLINE_POINTS], u8),
14    Borrowed(&'a [Vector]),
15}
16
17/// A point cloud with a radius, approximating a convex shape for sweep-based time-of-impact
18/// computations.
19///
20/// Only shapes that decompose exactly into a point cloud plus an inflation radius can be
21/// represented (balls, capsules, segments, triangles, cuboids, convex polygons/polyhedra and
22/// their round variants). Other shapes (cylinders, cones, half-spaces, composites, custom
23/// shapes) return `None` from [`ToiProxy::from_shape`].
24pub struct ToiProxy<'a> {
25    points: ProxyPoints<'a>,
26    /// The inflation radius around the point cloud.
27    pub radius: Real,
28}
29
30impl<'a> ToiProxy<'a> {
31    /// A proxy made of a single point with a radius.
32    pub fn point(point: Vector, radius: Real) -> Self {
33        let mut buf = [Vector::ZERO; TOI_PROXY_INLINE_POINTS];
34        buf[0] = point;
35        Self {
36            points: ProxyPoints::Inline(buf, 1),
37            radius,
38        }
39    }
40
41    /// A proxy borrowing its point cloud.
42    pub fn from_points(points: &'a [Vector], radius: Real) -> Self {
43        assert!(!points.is_empty());
44        Self {
45            points: ProxyPoints::Borrowed(points),
46            radius,
47        }
48    }
49
50    /// A proxy from an inline array of points.
51    pub fn from_array<const N: usize>(points: [Vector; N], radius: Real) -> Self {
52        assert!(N > 0 && N <= TOI_PROXY_INLINE_POINTS);
53        let mut buf = [Vector::ZERO; TOI_PROXY_INLINE_POINTS];
54        buf[..N].copy_from_slice(&points);
55        Self {
56            points: ProxyPoints::Inline(buf, N as u8),
57            radius,
58        }
59    }
60
61    /// Extracts a proxy from a shape, if the shape decomposes into points + radius.
62    pub fn from_shape(shape: &'a dyn Shape) -> Option<Self> {
63        match shape.as_typed_shape() {
64            TypedShape::Ball(ball) => Some(Self::point(Vector::ZERO, ball.radius)),
65            TypedShape::Cuboid(cuboid) => {
66                Some(Self::from_cuboid_half_extents(cuboid.half_extents, 0.0))
67            }
68            TypedShape::RoundCuboid(round) => Some(Self::from_cuboid_half_extents(
69                round.inner_shape.half_extents,
70                round.border_radius,
71            )),
72            TypedShape::Capsule(capsule) => Some(Self::from_array(
73                [capsule.segment.a, capsule.segment.b],
74                capsule.radius,
75            )),
76            TypedShape::Segment(segment) => Some(Self::from_array([segment.a, segment.b], 0.0)),
77            TypedShape::Triangle(tri) => Some(Self::from_array([tri.a, tri.b, tri.c], 0.0)),
78            TypedShape::RoundTriangle(round) => {
79                let tri = &round.inner_shape;
80                Some(Self::from_array([tri.a, tri.b, tri.c], round.border_radius))
81            }
82            #[cfg(feature = "dim2")]
83            #[cfg(feature = "alloc")]
84            TypedShape::ConvexPolygon(poly) => Some(Self::from_points(poly.points(), 0.0)),
85            #[cfg(feature = "dim2")]
86            #[cfg(feature = "alloc")]
87            TypedShape::RoundConvexPolygon(round) => Some(Self::from_points(
88                round.inner_shape.points(),
89                round.border_radius,
90            )),
91            #[cfg(feature = "dim3")]
92            #[cfg(feature = "alloc")]
93            TypedShape::ConvexPolyhedron(poly) => Some(Self::from_points(poly.points(), 0.0)),
94            #[cfg(feature = "dim3")]
95            #[cfg(feature = "alloc")]
96            TypedShape::RoundConvexPolyhedron(round) => Some(Self::from_points(
97                round.inner_shape.points(),
98                round.border_radius,
99            )),
100            _ => None,
101        }
102    }
103
104    fn from_cuboid_half_extents(he: Vector, radius: Real) -> Self {
105        #[cfg(feature = "dim2")]
106        {
107            Self::from_array(
108                [
109                    Vector::new(-he.x, -he.y),
110                    Vector::new(he.x, -he.y),
111                    Vector::new(he.x, he.y),
112                    Vector::new(-he.x, he.y),
113                ],
114                radius,
115            )
116        }
117        #[cfg(feature = "dim3")]
118        {
119            Self::from_array(
120                [
121                    Vector::new(-he.x, -he.y, -he.z),
122                    Vector::new(he.x, -he.y, -he.z),
123                    Vector::new(he.x, he.y, -he.z),
124                    Vector::new(-he.x, he.y, -he.z),
125                    Vector::new(-he.x, -he.y, he.z),
126                    Vector::new(he.x, -he.y, he.z),
127                    Vector::new(he.x, he.y, he.z),
128                    Vector::new(-he.x, he.y, he.z),
129                ],
130                radius,
131            )
132        }
133    }
134
135    /// The proxy’s point cloud.
136    #[inline]
137    pub fn points(&self) -> &[Vector] {
138        match &self.points {
139            ProxyPoints::Inline(buf, len) => &buf[..*len as usize],
140            ProxyPoints::Borrowed(points) => points,
141        }
142    }
143
144    /// The index of the proxy point with the greatest projection along `direction`.
145    ///
146    /// Projections are measured relative to the first point for better accuracy far from the
147    /// origin.
148    #[inline]
149    pub fn support(&self, direction: Vector) -> u32 {
150        let points = self.points();
151        let origin = points[0];
152        let mut best_index = 0;
153        let mut best_value = 0.0;
154        for (i, pt) in points.iter().enumerate().skip(1) {
155            let value = direction.dot(*pt - origin);
156            if value > best_value {
157                best_index = i;
158                best_value = value;
159            }
160        }
161        best_index as u32
162    }
163
164    /// The axis-aligned bounding box of this proxy under the given pose.
165    pub fn compute_aabb(&self, pose: &Pose) -> Aabb {
166        let points = self.points();
167        let mut mins = pose.transform_point(points[0]);
168        let mut maxs = mins;
169        for pt in &points[1..] {
170            let p = pose.transform_point(*pt);
171            mins = mins.min(p);
172            maxs = maxs.max(p);
173        }
174        Aabb::new(
175            mins - Vector::splat(self.radius),
176            maxs + Vector::splat(self.radius),
177        )
178    }
179}
parry3d/query/sweep_toi/toi_proxy.rs

parry3d/query/sweep_toi/
toi_proxy.rs
