avian2d/collision/collider/parry/
primitives2d.rs1use 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#[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#[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 let external_angle = self.external_angle_radians().adjust_precision();
253 let circumradius = self.circumradius().adjust_precision();
254
255 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 let n = (angle_from_top / external_angle).round() % self.sides as Scalar;
264
265 let target_angle = n * external_angle + FRAC_PI_2;
267
268 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 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 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 let target_angle1 = n1 * external_angle + FRAC_PI_2;
297 let target_angle2 = n2 * external_angle + FRAC_PI_2;
298
299 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}