bevy_heavy/dim2/
impls.rs

1use super::{ComputeMassProperties2d, MassProperties2d};
2use bevy_math::{
3    ops,
4    primitives::{
5        Annulus, Arc2d, Capsule2d, Circle, CircularSector, CircularSegment, ConvexPolygon, Ellipse,
6        Line2d, Measured2d, Plane2d, Polyline2d, Rectangle, RegularPolygon, Rhombus, Segment2d,
7        Triangle2d,
8    },
9    FloatPow, Vec2,
10};
11
12impl ComputeMassProperties2d for Circle {
13    #[inline]
14    fn mass(&self, density: f32) -> f32 {
15        self.area() * density
16    }
17
18    #[inline]
19    fn unit_angular_inertia(&self) -> f32 {
20        self.radius.squared() / 2.0
21    }
22
23    #[inline]
24    fn center_of_mass(&self) -> Vec2 {
25        Vec2::ZERO
26    }
27}
28
29impl ComputeMassProperties2d for CircularSector {
30    #[inline]
31    fn mass(&self, density: f32) -> f32 {
32        self.area() * density
33    }
34
35    #[inline]
36    fn unit_angular_inertia(&self) -> f32 {
37        0.5 * ops::powf(self.radius(), 4.0) * self.angle()
38    }
39
40    #[inline]
41    fn center_of_mass(&self) -> Vec2 {
42        let angle = self.angle();
43        let y = 2.0 * self.radius() * ops::sin(angle) / (3.0 * angle);
44        Vec2::new(0.0, y)
45    }
46}
47
48impl ComputeMassProperties2d for CircularSegment {
49    #[inline]
50    fn mass(&self, density: f32) -> f32 {
51        self.area() * density
52    }
53
54    #[inline]
55    fn unit_angular_inertia(&self) -> f32 {
56        let angle = self.angle();
57        let (sin, cos) = ops::sin_cos(angle);
58        ops::powf(self.radius(), 4.0) / 4.0 * (angle - sin + 2.0 / 3.0 * sin * (1.0 - cos) / 2.0)
59    }
60
61    #[inline]
62    fn center_of_mass(&self) -> Vec2 {
63        let y = self.radius() * ops::sin(self.half_angle()).cubed()
64            / (6.0 * self.half_angle() - ops::sin(self.angle()));
65        Vec2::new(0.0, y)
66    }
67}
68
69impl ComputeMassProperties2d for Ellipse {
70    #[inline]
71    fn mass(&self, density: f32) -> f32 {
72        self.area() * density
73    }
74
75    #[inline]
76    fn unit_angular_inertia(&self) -> f32 {
77        self.half_size.length_squared() / 4.0
78    }
79
80    #[inline]
81    fn center_of_mass(&self) -> Vec2 {
82        Vec2::ZERO
83    }
84}
85
86impl ComputeMassProperties2d for Annulus {
87    #[inline]
88    fn mass(&self, density: f32) -> f32 {
89        self.area() * density
90    }
91
92    #[inline]
93    fn unit_angular_inertia(&self) -> f32 {
94        0.5 * (self.outer_circle.radius.squared() + self.inner_circle.radius.squared())
95    }
96
97    #[inline]
98    fn center_of_mass(&self) -> Vec2 {
99        Vec2::ZERO
100    }
101}
102
103impl ComputeMassProperties2d for Triangle2d {
104    #[inline]
105    fn mass(&self, density: f32) -> f32 {
106        self.area() * density
107    }
108
109    #[inline]
110    fn unit_angular_inertia(&self) -> f32 {
111        // Adapted from Box2D: https://github.com/erincatto/box2d/blob/411acc32eb6d4f2e96fc70ddbdf01fe5f9b16230/src/collision/b2_polygon_shape.cpp#L274
112
113        // Note: The center of mass is used here, unlike in Box2D's or Parry's version.
114        let center_of_mass = self.center_of_mass();
115        let com_a = self.vertices[1] - center_of_mass;
116        let com_c = self.vertices[2] - center_of_mass;
117
118        (com_a.length_squared() + com_a.dot(com_c) + com_c.length_squared()) / 6.0
119    }
120
121    #[inline]
122    fn center_of_mass(&self) -> Vec2 {
123        (self.vertices[0] + self.vertices[1] + self.vertices[2]) / 3.0
124    }
125
126    #[inline]
127    fn mass_properties(&self, density: f32) -> MassProperties2d {
128        let area = self.area();
129        let center_of_mass = self.center_of_mass();
130
131        if area < f32::EPSILON {
132            return MassProperties2d::new(0.0, 0.0, center_of_mass);
133        }
134
135        let mass = area * density;
136
137        MassProperties2d::new(mass, self.angular_inertia(mass), center_of_mass)
138    }
139}
140
141impl ComputeMassProperties2d for Rectangle {
142    #[inline]
143    fn mass(&self, density: f32) -> f32 {
144        self.area() * density
145    }
146
147    #[inline]
148    fn unit_angular_inertia(&self) -> f32 {
149        self.half_size.length_squared() / 3.0
150    }
151
152    #[inline]
153    fn center_of_mass(&self) -> Vec2 {
154        Vec2::ZERO
155    }
156}
157
158impl ComputeMassProperties2d for Rhombus {
159    #[inline]
160    fn mass(&self, density: f32) -> f32 {
161        self.area() * density
162    }
163
164    #[inline]
165    fn unit_angular_inertia(&self) -> f32 {
166        self.half_diagonals.length_squared() / 12.0
167    }
168
169    #[inline]
170    fn center_of_mass(&self) -> Vec2 {
171        Vec2::ZERO
172    }
173}
174
175impl ComputeMassProperties2d for RegularPolygon {
176    #[inline]
177    fn mass(&self, density: f32) -> f32 {
178        self.area() * density
179    }
180
181    #[inline]
182    fn unit_angular_inertia(&self) -> f32 {
183        let half_external_angle = core::f32::consts::PI / self.sides as f32;
184        self.circumradius().squared() / 6.0 * (1.0 + 2.0 * ops::cos(half_external_angle).squared())
185    }
186
187    #[inline]
188    fn center_of_mass(&self) -> Vec2 {
189        Vec2::ZERO
190    }
191}
192
193impl ComputeMassProperties2d for Capsule2d {
194    #[inline]
195    fn mass(&self, density: f32) -> f32 {
196        let area = self.radius * (core::f32::consts::PI * self.radius + 4.0 * self.half_length);
197        area * density
198    }
199
200    #[inline]
201    fn unit_angular_inertia(&self) -> f32 {
202        // The rectangle and hemicircle parts
203        let rectangle = Rectangle {
204            half_size: Vec2::new(self.radius, self.half_length),
205        };
206        let rectangle_height = rectangle.half_size.y * 2.0;
207        let circle = Circle::new(self.radius);
208
209        // Areas
210        let rectangle_area = rectangle.area();
211        let circle_area = circle.area();
212
213        // Masses
214        let density = 1.0 / (rectangle_area + circle_area);
215        let rectangle_mass = rectangle_area * density;
216        let circle_mass = circle_area * density;
217
218        // Principal inertias
219        let rectangle_inertia = rectangle.angular_inertia(rectangle_mass);
220        let circle_inertia = circle.angular_inertia(circle_mass);
221
222        // Total inertia
223        let mut capsule_inertia = rectangle_inertia + circle_inertia;
224
225        // Compensate for the hemicircles being away from the rotation axis using the parallel axis theorem.
226        capsule_inertia += (rectangle_height.squared() * 0.25
227            + rectangle_height * self.radius * 3.0 / 8.0)
228            * circle_mass;
229
230        capsule_inertia
231    }
232
233    #[inline]
234    fn center_of_mass(&self) -> Vec2 {
235        Vec2::ZERO
236    }
237
238    #[inline]
239    fn mass_properties(&self, density: f32) -> MassProperties2d {
240        // The rectangle and hemicircle parts
241        let rectangle = Rectangle {
242            half_size: Vec2::new(self.radius, self.half_length),
243        };
244        let rectangle_height = rectangle.half_size.y * 2.0;
245        let circle = Circle::new(self.radius);
246
247        // Areas
248        let rectangle_area = rectangle.area();
249        let circle_area = circle.area();
250
251        // Masses
252        let rectangle_mass = rectangle_area * density;
253        let circle_mass = circle_area * density;
254
255        // Principal inertias
256        let rectangle_inertia = rectangle.angular_inertia(rectangle_mass);
257        let circle_inertia = circle.angular_inertia(circle_mass);
258
259        // Total inertia
260        let mut capsule_inertia = rectangle_inertia + circle_inertia;
261
262        // Compensate for the hemicircles being away from the rotation axis using the parallel axis theorem.
263        capsule_inertia += (rectangle_height.squared() * 0.25
264            + rectangle_height * self.radius * 3.0 / 8.0)
265            * circle_mass;
266
267        MassProperties2d::new(rectangle_mass + circle_mass, capsule_inertia, Vec2::ZERO)
268    }
269}
270
271impl ComputeMassProperties2d for ConvexPolygon {
272    #[inline]
273    fn mass(&self, density: f32) -> f32 {
274        let vertices = self.vertices();
275        let geometric_center =
276            vertices.iter().fold(Vec2::ZERO, |acc, vtx| acc + *vtx) / vertices.len() as f32;
277
278        // Initialize polygon area.
279        let mut area = 0.0;
280
281        // Create a peekable iterator over the polygon vertices.
282        let mut iter = vertices.iter().peekable();
283        let Some(first) = iter.peek().copied().copied() else {
284            return 0.0;
285        };
286
287        // Iterate through vertices, computing the sum of the areas of triangles.
288        // Each triangle is formed by the current vertex, next vertex, and the geometric center of the polygon.
289        while let Some(vertex) = iter.next() {
290            let (a, b, c) = (
291                *vertex,
292                iter.peek().copied().copied().unwrap_or(first),
293                geometric_center,
294            );
295            let tri_area = Triangle2d::new(a, b, c).area();
296
297            area += tri_area;
298        }
299
300        area * density
301    }
302
303    #[inline]
304    fn unit_angular_inertia(&self) -> f32 {
305        let vertices = self.vertices();
306        let (area, center_of_mass) = convex_polygon_area_and_com(vertices);
307
308        if area < f32::EPSILON {
309            return 0.0;
310        }
311
312        // Initialize polygon inertia.
313        let mut inertia = 0.0;
314
315        // Create a peekable iterator over the polygon vertices.
316        let mut iter = vertices.iter().peekable();
317        let first = **iter.peek().unwrap();
318
319        // Iterate through vertices, computing the sum of the areas of triangles.
320        // Each triangle is formed by the current vertex, next vertex, and the geometric center of the polygon.
321        while let Some(vertex) = iter.next() {
322            let triangle = Triangle2d::new(
323                *vertex,
324                iter.peek().copied().copied().unwrap_or(first),
325                center_of_mass,
326            );
327            inertia += triangle.unit_angular_inertia() * triangle.area();
328        }
329
330        inertia / area
331    }
332
333    #[inline]
334    fn center_of_mass(&self) -> Vec2 {
335        convex_polygon_area_and_com(self.vertices()).1
336    }
337
338    #[inline]
339    fn mass_properties(&self, density: f32) -> MassProperties2d {
340        let vertices = self.vertices();
341
342        // The polygon is assumed to be convex.
343        let (area, center_of_mass) = convex_polygon_area_and_com(vertices);
344
345        if area < f32::EPSILON {
346            return MassProperties2d::new(0.0, 0.0, center_of_mass);
347        }
348
349        // Initialize polygon inertia.
350        let mut inertia = 0.0;
351
352        // Create a peekable iterator over the polygon vertices.
353        let mut iter = vertices.iter().peekable();
354        let first = **iter.peek().unwrap();
355
356        // Iterate through vertices, computing the sum of the areas of triangles.
357        // Each triangle is formed by the current vertex, next vertex, and the geometric center of the polygon.
358        while let Some(vertex) = iter.next() {
359            let triangle = Triangle2d::new(
360                *vertex,
361                iter.peek().copied().copied().unwrap_or(first),
362                center_of_mass,
363            );
364            inertia += triangle.unit_angular_inertia() * triangle.area();
365        }
366
367        MassProperties2d::new(area * density, inertia * density, center_of_mass)
368    }
369}
370
371#[inline]
372fn convex_polygon_area_and_com(vertices: &[Vec2]) -> (f32, Vec2) {
373    let geometric_center =
374        vertices.iter().fold(Vec2::ZERO, |acc, vtx| acc + *vtx) / vertices.len() as f32;
375
376    // Initialize polygon area and center.
377    let mut area = 0.0;
378    let mut center = Vec2::ZERO;
379
380    // Create a peekable iterator over the polygon vertices.
381    let mut iter = vertices.iter().peekable();
382    let Some(first) = iter.peek().copied().copied() else {
383        return (0.0, Vec2::ZERO);
384    };
385
386    // Iterate through vertices, computing the sum of the areas and centers of triangles.
387    // Each triangle is formed by the current vertex, next vertex, and the geometric center of the polygon.
388    while let Some(vertex) = iter.next() {
389        let (a, b, c) = (
390            *vertex,
391            iter.peek().copied().copied().unwrap_or(first),
392            geometric_center,
393        );
394        let tri_area = Triangle2d::new(a, b, c).area();
395        let tri_center = (a + b + c) / 3.0;
396
397        area += tri_area;
398        center += tri_center * tri_area;
399    }
400
401    if area < f32::EPSILON {
402        (area, geometric_center)
403    } else {
404        (area, center / area)
405    }
406}
407
408macro_rules! impl_zero_mass_properties_2d {
409    ($($shape:ty),*) => {
410        $(
411            impl ComputeMassProperties2d for $shape {
412                #[inline]
413                fn mass(&self, _density: f32) -> f32 {
414                    0.0
415                }
416
417                #[inline]
418                fn unit_angular_inertia(&self) -> f32 {
419                    0.0
420                }
421
422                #[inline]
423                fn angular_inertia(&self, _mass: f32) -> f32 {
424                    0.0
425                }
426
427                #[inline]
428                fn center_of_mass(&self) -> Vec2 {
429                    Vec2::ZERO
430                }
431
432                #[inline]
433                fn mass_properties(&self, _density: f32) -> MassProperties2d {
434                    MassProperties2d::ZERO
435                }
436            }
437        )*
438    };
439}
440
441impl_zero_mass_properties_2d!(Arc2d);
442impl_zero_mass_properties_2d!(Plane2d);
443impl_zero_mass_properties_2d!(Line2d);
444impl_zero_mass_properties_2d!(Segment2d);
445impl_zero_mass_properties_2d!(Polyline2d);
446
447#[cfg(test)]
448mod tests {
449    use alloc::vec::Vec;
450
451    use approx::assert_relative_eq;
452    use bevy_math::ShapeSample;
453    use rand::SeedableRng;
454
455    use super::*;
456
457    macro_rules! test_shape {
458        ($test_name:tt, $shape:expr) => {
459            #[test]
460            fn $test_name() {
461                let shape = $shape;
462
463                // Sample enough points to have a close enough point cloud representation of the shape.
464                let mut rng = rand_chacha::ChaCha8Rng::from_seed(Default::default());
465                let points = (0..1_000_000)
466                    .map(|_| shape.sample_interior(&mut rng))
467                    .collect::<Vec<_>>();
468
469                // Compute the mass properties to test.
470                let density = 2.0;
471                let mass = shape.mass(density);
472                let angular_inertia = shape.angular_inertia(mass);
473                let center_of_mass = shape.center_of_mass();
474
475                // First, test that the individually computed properties match the full properties.
476                let mass_props = shape.mass_properties(density);
477                assert_relative_eq!(mass, mass_props.mass);
478                assert_relative_eq!(angular_inertia, mass_props.angular_inertia);
479                assert_relative_eq!(center_of_mass, mass_props.center_of_mass);
480
481                // Estimate the expected mass properties using the point cloud.
482                // Note: We could also approximate the mass using Monte Carlo integration.
483                //       This would require point containment checks.
484                let expected = MassProperties2d::from_point_cloud(&points, mass);
485
486                assert_relative_eq!(mass, expected.mass);
487                assert_relative_eq!(angular_inertia, expected.angular_inertia, epsilon = 0.1);
488                assert_relative_eq!(center_of_mass, expected.center_of_mass, epsilon = 0.01);
489            }
490        };
491    }
492
493    // TODO: Test randomized shape definitions.
494
495    test_shape!(circle, Circle::new(2.0));
496    // test_shape!(circular_sector, CircularSector::new(2.0, TAU));
497    // test_shape!(circular_segment, CircularSegment::new(2.0, TAU));
498    // test_shape!(ellipse, Ellipse::new(2.0, 1.0));
499    test_shape!(annulus, Annulus::new(1.0, 2.0));
500    test_shape!(
501        triangle,
502        Triangle2d::new(
503            Vec2::new(8.0, 6.0),
504            Vec2::new(2.0, 0.0),
505            Vec2::new(6.0, 2.0)
506        )
507    );
508    test_shape!(rectangle, Rectangle::new(2.0, 1.0));
509    // test_shape!(rhombus, Rhombus::new(2.0, 1.0));
510    // test_shape!(regular_polygon, RegularPolygon::new(2.0, 6));
511    // test_shape!(polygon, Polygon::new([Vec2::ZERO, Vec2::X, Vec2::Y]));
512    test_shape!(capsule, Capsule2d::new(1.0, 0.25));
513}