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

avian2d/collision/collider/parry/
primitives2d.rs
