Implement RegularPolygon colliders with a custom shape (#367)

# Objective

Currently, regular polygons don't have their own collider shape. Instead, the `From<RegularPolygon>` implementation computes the convex hull and creates a convex polygon from that. This may not be the most efficient approach however, as some computations can be simpler and more efficient if the polygon is known to be regular.

This PR adds a custom shape implementation for the 2D `RegularPolygon` primitive.

## Solution

Add a `RegularPolygonWrapper` type that is used as the internal representation for the custom regular polygon shape. The custom shape uses the ID `2`.

---

## Migration Guide

If you had a custom Parry shape using the ID `2`, you might need to change it, because the ID is used for regular polygons.

src/math/double.rs

@@ -3,8 +3,12 @@ use bevy_math::*;
 
 /// The floating point number type used by Bevy XPBD.
 pub type Scalar = f64;
+/// The PI/2 constant.
+pub const FRAC_PI_2: Scalar = std::f64::consts::FRAC_PI_2;
 /// The PI constant.
 pub const PI: Scalar = std::f64::consts::PI;
+/// The TAU constant.
+pub const TAU: Scalar = std::f64::consts::TAU;
 
 /// The vector type used by Bevy XPBD.
 #[cfg(feature = "2d")]

src/math/single.rs

@@ -3,8 +3,12 @@ use bevy_math::*;
 
 /// The floating point number type used by Bevy XPBD.
 pub type Scalar = f32;
+/// The PI/2 constant.
+pub const FRAC_PI_2: Scalar = std::f32::consts::FRAC_PI_2;
 /// The PI constant.
 pub const PI: Scalar = std::f32::consts::PI;
+/// The TAU constant.
+pub const TAU: Scalar = std::f32::consts::TAU;
 
 /// The vector type used by Bevy XPBD.
 #[cfg(feature = "2d")]

src/plugins/collision/collider/parry/mod.rs

@@ -14,7 +14,7 @@ mod primitives2d;
 mod primitives3d;
 
 #[cfg(feature = "2d")]
-pub(crate) use primitives2d::EllipseWrapper;
+pub(crate) use primitives2d::{EllipseWrapper, RegularPolygonWrapper};
 
 impl<T: IntoCollider<Collider>> From<T> for Collider {
     fn from(value: T) -> Self {
@@ -1105,6 +1105,29 @@ fn scale_shape(
                         half_size: ellipse.half_size * scale.f32(),
                     })));
                 }
+            } else if _id == 2 {
+                if let Some(polygon) = shape.as_shape::<RegularPolygonWrapper>() {
+                    if scale.x == scale.y {
+                        return Ok(SharedShape::new(RegularPolygonWrapper(
+                            RegularPolygon::new(
+                                polygon.circumradius() * scale.x as f32,
+                                polygon.sides,
+                            ),
+                        )));
+                    } else {
+                        let vertices = polygon
+                            .vertices(0.0)
+                            .into_iter()
+                            .map(|v| v.adjust_precision().into())
+                            .collect::<Vec<_>>();
+
+                        return scale_shape(
+                            &SharedShape::convex_hull(&vertices).unwrap(),
+                            scale,
+                            num_subdivisions,
+                        );
+                    }
+                }
             }
             Err(parry::query::Unsupported)
         }

src/plugins/collision/collider/parry/primitives2d.rs

@@ -1,17 +1,20 @@
-use crate::{AdjustPrecision, AsF32, Scalar, Vector};
+use crate::{AdjustPrecision, AsF32, Scalar, Vector, FRAC_PI_2, PI, TAU};
 
 use super::{Collider, IntoCollider};
 use bevy::prelude::Deref;
 use bevy_math::{bounding::Bounded2d, prelude::*};
-use nalgebra::{Point2, Vector2};
+use nalgebra::{Point2, UnitVector2, Vector2};
 use parry::{
     mass_properties::MassProperties,
     math::Isometry,
     query::{
         details::local_ray_intersection_with_support_map_with_params, gjk::VoronoiSimplex,
         point::local_point_projection_on_support_map, PointQuery, RayCast,
     },
-    shape::{FeatureId, Shape, SharedShape, SupportMap},
+    shape::{
+        FeatureId, PackedFeatureId, PolygonalFeature, PolygonalFeatureMap, Shape, SharedShape,
+        SupportMap,
+    },
 };
 
 impl IntoCollider<Collider> for Circle {
@@ -224,12 +227,223 @@ impl IntoCollider<Collider> for BoxedPolygon {
 
 impl IntoCollider<Collider> for RegularPolygon {
     fn collider(&self) -> Collider {
-        let vertices = self
-            .vertices(0.0)
-            .into_iter()
-            .map(|v| v.adjust_precision())
-            .collect();
-        Collider::convex_hull(vertices).unwrap()
+        Collider::from(SharedShape::new(RegularPolygonWrapper(*self)))
+    }
+}
+
+#[derive(Clone, Copy, Debug, Deref)]
+pub(crate) struct RegularPolygonWrapper(pub(crate) RegularPolygon);
+
+impl SupportMap for RegularPolygonWrapper {
+    #[inline]
+    fn local_support_point(&self, direction: &Vector2<Scalar>) -> Point2<Scalar> {
+        // TODO: For polygons with a small number of sides, maybe just iterating
+        //       through the vertices and comparing dot products is faster?
+
+        let external_angle = self.external_angle_radians().adjust_precision();
+        let circumradius = self.circumradius().adjust_precision();
+
+        // Counterclockwise
+        let angle_from_top = if direction.x < 0.0 {
+            -Vector::from(*direction).angle_between(Vector::Y)
+        } else {
+            TAU - Vector::from(*direction).angle_between(Vector::Y)
+        };
+
+        // How many rotations of `external_angle` correspond to the vertex closest to the support direction.
+        let n = (angle_from_top / external_angle).round() % self.sides as Scalar;
+
+        // Rotate by an additional 90 degrees so that the first vertex is always at the top.
+        let target_angle = n * external_angle + FRAC_PI_2;
+
+        // Compute the vertex corresponding to the target angle on the unit circle.
+        Point2::from(circumradius * Vector::from_angle(target_angle))
+    }
+}
+
+impl PolygonalFeatureMap for RegularPolygonWrapper {
+    #[inline]
+    fn local_support_feature(
+        &self,
+        direction: &UnitVector2<Scalar>,
+        out_feature: &mut PolygonalFeature,
+    ) {
+        let external_angle = self.external_angle_radians().adjust_precision();
+        let circumradius = self.circumradius().adjust_precision();
+
+        // Counterclockwise
+        let angle_from_top = if direction.x < 0.0 {
+            -Vector::from(*direction).angle_between(Vector::Y)
+        } else {
+            TAU - Vector::from(*direction).angle_between(Vector::Y)
+        };
+
+        // How many rotations of `external_angle` correspond to the vertices.
+        let n_unnormalized = angle_from_top / external_angle;
+        let n1 = n_unnormalized.floor() % self.sides as Scalar;
+        let n2 = n_unnormalized.ceil() % self.sides as Scalar;
+
+        // Rotate by an additional 90 degrees so that the first vertex is always at the top.
+        let target_angle1 = n1 * external_angle + FRAC_PI_2;
+        let target_angle2 = n2 * external_angle + FRAC_PI_2;
+
+        // Compute the vertices corresponding to the target angle on the unit circle.
+        let vertex1 = Point2::from(circumradius * Vector::from_angle(target_angle1));
+        let vertex2 = Point2::from(circumradius * Vector::from_angle(target_angle2));
+
+        *out_feature = PolygonalFeature {
+            vertices: [vertex1, vertex2],
+            vids: [
+                PackedFeatureId::vertex(n1 as u32),
+                PackedFeatureId::vertex(n2 as u32),
+            ],
+            fid: PackedFeatureId::face(n1 as u32),
+            num_vertices: 2,
+        };
+    }
+}
+
+impl Shape for RegularPolygonWrapper {
+    fn compute_local_aabb(&self) -> parry::bounding_volume::Aabb {
+        let aabb = self.aabb_2d(Vec2::ZERO, 0.0);
+        parry::bounding_volume::Aabb::new(
+            aabb.min.adjust_precision().into(),
+            aabb.max.adjust_precision().into(),
+        )
+    }
+
+    fn compute_aabb(&self, position: &Isometry<Scalar>) -> parry::bounding_volume::Aabb {
+        let aabb = self.aabb_2d(
+            Vector::from(position.translation).f32(),
+            position.rotation.angle() as f32,
+        );
+        parry::bounding_volume::Aabb::new(
+            aabb.min.adjust_precision().into(),
+            aabb.max.adjust_precision().into(),
+        )
+    }
+
+    fn compute_local_bounding_sphere(&self) -> parry::bounding_volume::BoundingSphere {
+        let sphere = self.bounding_circle(Vec2::ZERO, 0.0);
+        parry::bounding_volume::BoundingSphere::new(
+            sphere.center.adjust_precision().into(),
+            sphere.radius().adjust_precision(),
+        )
+    }
+
+    fn compute_bounding_sphere(
+        &self,
+        position: &Isometry<Scalar>,
+    ) -> parry::bounding_volume::BoundingSphere {
+        let sphere = self.bounding_circle(
+            Vector::from(position.translation).f32(),
+            position.rotation.angle() as f32,
+        );
+        parry::bounding_volume::BoundingSphere::new(
+            sphere.center.adjust_precision().into(),
+            sphere.radius().adjust_precision(),
+        )
+    }
+
+    fn clone_box(&self) -> Box<dyn Shape> {
+        Box::new(*self)
+    }
+
+    fn mass_properties(&self, density: Scalar) -> MassProperties {
+        let volume = self.area().adjust_precision();
+        let mass = volume * density;
+
+        let half_external_angle = PI / self.sides as Scalar;
+        let angular_inertia = mass * self.circumradius().adjust_precision().powi(2) / 6.0
+            * (1.0 + 2.0 * half_external_angle.cos().powi(2));
+
+        MassProperties::new(Point2::origin(), mass, angular_inertia)
+    }
+
+    fn is_convex(&self) -> bool {
+        true
+    }
+
+    fn shape_type(&self) -> parry::shape::ShapeType {
+        parry::shape::ShapeType::Custom
+    }
+
+    fn as_typed_shape(&self) -> parry::shape::TypedShape {
+        parry::shape::TypedShape::Custom(2)
+    }
+
+    fn ccd_thickness(&self) -> Scalar {
+        self.circumradius().adjust_precision()
+    }
+
+    fn ccd_angular_thickness(&self) -> Scalar {
+        crate::math::PI - self.internal_angle_radians().adjust_precision()
+    }
+
+    fn as_support_map(&self) -> Option<&dyn SupportMap> {
+        Some(self as &dyn SupportMap)
+    }
+
+    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, Scalar)> {
+        Some((self as &dyn PolygonalFeatureMap, 0.0))
+    }
+
+    fn feature_normal_at_point(
+        &self,
+        feature: FeatureId,
+        _point: &Point2<Scalar>,
+    ) -> Option<UnitVector2<Scalar>> {
+        match feature {
+            FeatureId::Face(id) => {
+                let external_angle = self.external_angle_radians().adjust_precision();
+                let normal_angle = id as Scalar * external_angle - external_angle * 0.5 + FRAC_PI_2;
+                Some(UnitVector2::new_unchecked(
+                    Vector::from_angle(normal_angle).into(),
+                ))
+            }
+            FeatureId::Vertex(id) => {
+                let external_angle = self.external_angle_radians().adjust_precision();
+                let normal_angle = id as Scalar * external_angle + FRAC_PI_2;
+                Some(UnitVector2::new_unchecked(
+                    Vector::from_angle(normal_angle).into(),
+                ))
+            }
+            _ => None,
+        }
+    }
+}
+
+impl RayCast for RegularPolygonWrapper {
+    fn cast_local_ray_and_get_normal(
+        &self,
+        ray: &parry::query::Ray,
+        max_toi: Scalar,
+        solid: bool,
+    ) -> Option<parry::query::RayIntersection> {
+        local_ray_intersection_with_support_map_with_params(
+            self,
+            &mut VoronoiSimplex::new(),
+            ray,
+            max_toi,
+            solid,
+        )
+    }
+}
+
+impl PointQuery for RegularPolygonWrapper {
+    fn project_local_point(
+        &self,
+        pt: &parry::math::Point<Scalar>,
+        solid: bool,
+    ) -> parry::query::PointProjection {
+        local_point_projection_on_support_map(self, &mut VoronoiSimplex::new(), pt, solid)
+    }
+
+    fn project_local_point_and_get_feature(
+        &self,
+        pt: &parry::math::Point<Scalar>,
+    ) -> (parry::query::PointProjection, parry::shape::FeatureId) {
+        (self.project_local_point(pt, false), FeatureId::Unknown)
     }
 }
 

src/plugins/debug/gizmos.rs

@@ -445,6 +445,17 @@ impl<'w, 's> PhysicsGizmoExt for Gizmos<'w, 's, PhysicsGizmos> {
                             color,
                         );
                     }
+                } else if _id == 2 {
+                    if let Some(polygon) =
+                        collider.shape_scaled().as_shape::<RegularPolygonWrapper>()
+                    {
+                        self.primitive_2d(
+                            polygon.0,
+                            position.f32(),
+                            rotation.as_radians() as f32,
+                            color,
+                        );
+                    }
                 }
             }
         }