avian2d/collision/collider/parry/
primitives2d.rs

1use crate::{AdjustPrecision, FRAC_PI_2, PI, Scalar, TAU, Vector, math};
2
3use super::{AsF32, Collider, IntoCollider};
4use bevy::prelude::{Deref, DerefMut};
5use bevy_math::{bounding::Bounded2d, prelude::*};
6use parry::{
7    mass_properties::MassProperties,
8    math::Pose,
9    query::{
10        PointQuery, RayCast, details::local_ray_intersection_with_support_map_with_params,
11        gjk::VoronoiSimplex, point::local_point_projection_on_support_map,
12    },
13    shape::{
14        FeatureId, PackedFeatureId, PolygonalFeature, PolygonalFeatureMap, Shape, SharedShape,
15        SupportMap,
16    },
17};
18
19impl IntoCollider<Collider> for Circle {
20    fn collider(&self) -> Collider {
21        Collider::circle(self.radius.adjust_precision())
22    }
23}
24
25impl IntoCollider<Collider> for Ellipse {
26    fn collider(&self) -> Collider {
27        Collider::from(SharedShape::new(EllipseColliderShape(*self)))
28    }
29}
30
31/// An ellipse shape that can be stored in a [`SharedShape`] for an ellipse [`Collider`].
32///
33/// This wrapper is required to allow implementing the necessary traits from [`parry`]
34/// for Bevy's [`Ellipse`] type.
35#[derive(Clone, Copy, Debug, Deref, DerefMut)]
36pub struct EllipseColliderShape(pub Ellipse);
37
38impl SupportMap for EllipseColliderShape {
39    #[inline]
40    fn local_support_point(&self, direction: Vector) -> Vector {
41        let [a, b] = self.half_size.adjust_precision().to_array();
42        let denom = (direction.x.powi(2) * a * a + direction.y.powi(2) * b * b).sqrt();
43        Vector::new(a * a * direction.x / denom, b * b * direction.y / denom)
44    }
45}
46
47impl Shape for EllipseColliderShape {
48    fn clone_dyn(&self) -> Box<dyn Shape> {
49        Box::new(*self)
50    }
51
52    fn scale_dyn(
53        &self,
54        scale: Vector,
55        _num_subdivisions: u32,
56    ) -> Option<Box<dyn parry::shape::Shape>> {
57        let half_size = scale.f32() * self.half_size;
58        Some(Box::new(EllipseColliderShape(Ellipse::new(
59            half_size.x,
60            half_size.y,
61        ))))
62    }
63
64    fn compute_local_aabb(&self) -> parry::bounding_volume::Aabb {
65        let aabb = self.aabb_2d(Isometry2d::IDENTITY);
66        parry::bounding_volume::Aabb::new(aabb.min.adjust_precision(), aabb.max.adjust_precision())
67    }
68
69    fn compute_aabb(&self, position: &Pose) -> parry::bounding_volume::Aabb {
70        let isometry = math::pose_to_isometry(position);
71        let aabb = self.aabb_2d(isometry);
72        parry::bounding_volume::Aabb::new(aabb.min.adjust_precision(), aabb.max.adjust_precision())
73    }
74
75    fn compute_local_bounding_sphere(&self) -> parry::bounding_volume::BoundingSphere {
76        let sphere = self.bounding_circle(Isometry2d::IDENTITY);
77        parry::bounding_volume::BoundingSphere::new(
78            sphere.center.adjust_precision(),
79            sphere.radius().adjust_precision(),
80        )
81    }
82
83    fn compute_bounding_sphere(&self, position: &Pose) -> parry::bounding_volume::BoundingSphere {
84        let isometry = math::pose_to_isometry(position);
85        let sphere = self.bounding_circle(isometry);
86        parry::bounding_volume::BoundingSphere::new(
87            sphere.center.adjust_precision(),
88            sphere.radius().adjust_precision(),
89        )
90    }
91
92    fn clone_box(&self) -> Box<dyn Shape> {
93        Box::new(*self)
94    }
95
96    fn mass_properties(&self, density: Scalar) -> MassProperties {
97        let volume = self.area().adjust_precision();
98        let mass = volume * density;
99        let inertia = mass * self.half_size.length_squared().adjust_precision() / 4.0;
100        MassProperties::new(Vector::ZERO, mass, inertia)
101    }
102
103    fn is_convex(&self) -> bool {
104        true
105    }
106
107    fn shape_type(&self) -> parry::shape::ShapeType {
108        parry::shape::ShapeType::Custom
109    }
110
111    fn as_typed_shape(&self) -> parry::shape::TypedShape<'_> {
112        parry::shape::TypedShape::Custom(self)
113    }
114
115    fn ccd_thickness(&self) -> Scalar {
116        self.half_size.max_element().adjust_precision()
117    }
118
119    fn ccd_angular_thickness(&self) -> Scalar {
120        crate::math::PI
121    }
122
123    fn as_support_map(&self) -> Option<&dyn SupportMap> {
124        Some(self as &dyn SupportMap)
125    }
126}
127
128impl RayCast for EllipseColliderShape {
129    fn cast_local_ray_and_get_normal(
130        &self,
131        ray: &parry::query::Ray,
132        max_toi: Scalar,
133        solid: bool,
134    ) -> Option<parry::query::RayIntersection> {
135        local_ray_intersection_with_support_map_with_params(
136            self,
137            &mut VoronoiSimplex::new(),
138            ray,
139            max_toi,
140            solid,
141        )
142    }
143}
144
145impl PointQuery for EllipseColliderShape {
146    fn project_local_point(&self, pt: Vector, solid: bool) -> parry::query::PointProjection {
147        local_point_projection_on_support_map(self, &mut VoronoiSimplex::new(), pt, solid)
148    }
149
150    fn project_local_point_and_get_feature(
151        &self,
152        pt: Vector,
153    ) -> (parry::query::PointProjection, parry::shape::FeatureId) {
154        (self.project_local_point(pt, false), FeatureId::Unknown)
155    }
156}
157
158impl IntoCollider<Collider> for Plane2d {
159    fn collider(&self) -> Collider {
160        let vec = self.normal.perp().adjust_precision() * 100_000.0 / 2.0;
161        Collider::segment(-vec, vec)
162    }
163}
164
165impl IntoCollider<Collider> for Line2d {
166    fn collider(&self) -> Collider {
167        let vec = self.direction.adjust_precision() * 100_000.0 / 2.0;
168        Collider::segment(-vec, vec)
169    }
170}
171
172impl IntoCollider<Collider> for Segment2d {
173    fn collider(&self) -> Collider {
174        let (point1, point2) = (self.point1(), self.point2());
175        Collider::segment(point1.adjust_precision(), point2.adjust_precision())
176    }
177}
178
179impl IntoCollider<Collider> for Triangle2d {
180    fn collider(&self) -> Collider {
181        Collider::triangle(
182            self.vertices[0].adjust_precision(),
183            self.vertices[1].adjust_precision(),
184            self.vertices[2].adjust_precision(),
185        )
186    }
187}
188
189impl IntoCollider<Collider> for Rectangle {
190    fn collider(&self) -> Collider {
191        Collider::from(SharedShape::cuboid(
192            self.half_size.x.adjust_precision(),
193            self.half_size.y.adjust_precision(),
194        ))
195    }
196}
197
198impl IntoCollider<Collider> for Polygon {
199    fn collider(&self) -> Collider {
200        let vertices = self.vertices.iter().map(|v| v.adjust_precision()).collect();
201        let indices = (0..self.vertices.len() as u32 - 1)
202            .map(|i| [i, i + 1])
203            .collect();
204        Collider::convex_decomposition(vertices, indices)
205    }
206}
207
208impl IntoCollider<Collider> for ConvexPolygon {
209    fn collider(&self) -> Collider {
210        let vertices = self
211            .vertices()
212            .iter()
213            .map(|v| v.adjust_precision())
214            .collect();
215        Collider::convex_polyline(vertices).unwrap()
216    }
217}
218
219impl IntoCollider<Collider> for RegularPolygon {
220    fn collider(&self) -> Collider {
221        Collider::from(SharedShape::new(RegularPolygonColliderShape(*self)))
222    }
223}
224
225/// A regular polygon shape that can be stored in a [`SharedShape`] for a regular polygon [`Collider`].
226///
227/// This wrapper is required to allow implementing the necessary traits from [`parry`]
228/// for Bevy's [`RegularPolygon`] type.
229#[derive(Clone, Copy, Debug, Deref, DerefMut)]
230pub struct RegularPolygonColliderShape(pub RegularPolygon);
231
232impl SupportMap for RegularPolygonColliderShape {
233    #[inline]
234    fn local_support_point(&self, direction: Vector) -> Vector {
235        // TODO: For polygons with a small number of sides, maybe just iterating
236        //       through the vertices and comparing dot products is faster?
237
238        let external_angle = self.external_angle_radians().adjust_precision();
239        let circumradius = self.circumradius().adjust_precision();
240
241        // Counterclockwise
242        let angle_from_top = if direction.x < 0.0 {
243            -direction.angle_to(Vector::Y)
244        } else {
245            TAU - direction.angle_to(Vector::Y)
246        };
247
248        // How many rotations of `external_angle` correspond to the vertex closest to the support direction.
249        let n = (angle_from_top / external_angle).round() % self.sides as Scalar;
250
251        // Rotate by an additional 90 degrees so that the first vertex is always at the top.
252        let target_angle = n * external_angle + FRAC_PI_2;
253
254        // Compute the vertex corresponding to the target angle on the unit circle.
255        circumradius * Vector::from_angle(target_angle)
256    }
257}
258
259impl PolygonalFeatureMap for RegularPolygonColliderShape {
260    #[inline]
261    fn local_support_feature(&self, direction: Vector, out_feature: &mut PolygonalFeature) {
262        let external_angle = self.external_angle_radians().adjust_precision();
263        let circumradius = self.circumradius().adjust_precision();
264
265        // Counterclockwise
266        let angle_from_top = if direction.x < 0.0 {
267            -direction.angle_to(Vector::Y)
268        } else {
269            TAU - direction.angle_to(Vector::Y)
270        };
271
272        // How many rotations of `external_angle` correspond to the vertices.
273        let n_unnormalized = angle_from_top / external_angle;
274        let n1 = n_unnormalized.floor() % self.sides as Scalar;
275        let n2 = n_unnormalized.ceil() % self.sides as Scalar;
276
277        // Rotate by an additional 90 degrees so that the first vertex is always at the top.
278        let target_angle1 = n1 * external_angle + FRAC_PI_2;
279        let target_angle2 = n2 * external_angle + FRAC_PI_2;
280
281        // Compute the vertices corresponding to the target angle on the unit circle.
282        let vertex1 = circumradius * Vector::from_angle(target_angle1);
283        let vertex2 = circumradius * Vector::from_angle(target_angle2);
284
285        *out_feature = PolygonalFeature {
286            vertices: [vertex1, vertex2],
287            vids: [
288                PackedFeatureId::vertex(n1 as u32),
289                PackedFeatureId::vertex(n2 as u32),
290            ],
291            fid: PackedFeatureId::face(n1 as u32),
292            num_vertices: 2,
293        };
294    }
295}
296
297impl Shape for RegularPolygonColliderShape {
298    fn clone_dyn(&self) -> Box<dyn Shape> {
299        Box::new(*self)
300    }
301
302    fn scale_dyn(
303        &self,
304        scale: Vector,
305        _num_subdivisions: u32,
306    ) -> Option<Box<dyn parry::shape::Shape>> {
307        let circumradius = scale.f32() * self.circumradius();
308        Some(Box::new(RegularPolygonColliderShape(RegularPolygon::new(
309            circumradius.length(),
310            self.sides,
311        ))))
312    }
313
314    fn compute_local_aabb(&self) -> parry::bounding_volume::Aabb {
315        let aabb = self.aabb_2d(Isometry2d::IDENTITY);
316        parry::bounding_volume::Aabb::new(aabb.min.adjust_precision(), aabb.max.adjust_precision())
317    }
318
319    fn compute_aabb(&self, position: &Pose) -> parry::bounding_volume::Aabb {
320        let isometry = math::pose_to_isometry(position);
321        let aabb = self.aabb_2d(isometry);
322        parry::bounding_volume::Aabb::new(aabb.min.adjust_precision(), aabb.max.adjust_precision())
323    }
324
325    fn compute_local_bounding_sphere(&self) -> parry::bounding_volume::BoundingSphere {
326        let sphere = self.bounding_circle(Isometry2d::IDENTITY);
327        parry::bounding_volume::BoundingSphere::new(
328            sphere.center.adjust_precision(),
329            sphere.radius().adjust_precision(),
330        )
331    }
332
333    fn compute_bounding_sphere(&self, position: &Pose) -> parry::bounding_volume::BoundingSphere {
334        let isometry = math::pose_to_isometry(position);
335        let sphere = self.bounding_circle(isometry);
336        parry::bounding_volume::BoundingSphere::new(
337            sphere.center.adjust_precision(),
338            sphere.radius().adjust_precision(),
339        )
340    }
341
342    fn clone_box(&self) -> Box<dyn Shape> {
343        Box::new(*self)
344    }
345
346    fn mass_properties(&self, density: Scalar) -> MassProperties {
347        let volume = self.area().adjust_precision();
348        let mass = volume * density;
349
350        let half_external_angle = PI / self.sides as Scalar;
351        let angular_inertia = mass * self.circumradius().adjust_precision().powi(2) / 6.0
352            * (1.0 + 2.0 * half_external_angle.cos().powi(2));
353
354        MassProperties::new(Vector::ZERO, mass, angular_inertia)
355    }
356
357    fn is_convex(&self) -> bool {
358        true
359    }
360
361    fn shape_type(&self) -> parry::shape::ShapeType {
362        parry::shape::ShapeType::Custom
363    }
364
365    fn as_typed_shape(&self) -> parry::shape::TypedShape<'_> {
366        parry::shape::TypedShape::Custom(self)
367    }
368
369    fn ccd_thickness(&self) -> Scalar {
370        self.circumradius().adjust_precision()
371    }
372
373    fn ccd_angular_thickness(&self) -> Scalar {
374        crate::math::PI - self.internal_angle_radians().adjust_precision()
375    }
376
377    fn as_support_map(&self) -> Option<&dyn SupportMap> {
378        Some(self as &dyn SupportMap)
379    }
380
381    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, Scalar)> {
382        Some((self as &dyn PolygonalFeatureMap, 0.0))
383    }
384
385    fn feature_normal_at_point(&self, feature: FeatureId, _point: Vector) -> Option<Vector> {
386        match feature {
387            FeatureId::Face(id) => {
388                let external_angle = self.external_angle_radians().adjust_precision();
389                let normal_angle = id as Scalar * external_angle - external_angle * 0.5 + FRAC_PI_2;
390                Some(Vector::from_angle(normal_angle))
391            }
392            FeatureId::Vertex(id) => {
393                let external_angle = self.external_angle_radians().adjust_precision();
394                let normal_angle = id as Scalar * external_angle + FRAC_PI_2;
395                Some(Vector::from_angle(normal_angle))
396            }
397            _ => None,
398        }
399    }
400}
401
402impl RayCast for RegularPolygonColliderShape {
403    fn cast_local_ray_and_get_normal(
404        &self,
405        ray: &parry::query::Ray,
406        max_toi: Scalar,
407        solid: bool,
408    ) -> Option<parry::query::RayIntersection> {
409        local_ray_intersection_with_support_map_with_params(
410            self,
411            &mut VoronoiSimplex::new(),
412            ray,
413            max_toi,
414            solid,
415        )
416    }
417}
418
419impl PointQuery for RegularPolygonColliderShape {
420    fn project_local_point(&self, pt: Vector, solid: bool) -> parry::query::PointProjection {
421        local_point_projection_on_support_map(self, &mut VoronoiSimplex::new(), pt, solid)
422    }
423
424    fn project_local_point_and_get_feature(
425        &self,
426        pt: Vector,
427    ) -> (parry::query::PointProjection, parry::shape::FeatureId) {
428        (self.project_local_point(pt, false), FeatureId::Unknown)
429    }
430}
431
432impl IntoCollider<Collider> for Capsule2d {
433    fn collider(&self) -> Collider {
434        Collider::capsule(
435            self.radius.adjust_precision(),
436            2.0 * self.half_length.adjust_precision(),
437        )
438    }
439}
avian2d/collision/collider/parry/primitives2d.rs

avian2d/collision/collider/parry/
primitives2d.rs
