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 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 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 let rectangle_area = rectangle.area();
211 let circle_area = circle.area();
212
213 let density = 1.0 / (rectangle_area + circle_area);
215 let rectangle_mass = rectangle_area * density;
216 let circle_mass = circle_area * density;
217
218 let rectangle_inertia = rectangle.angular_inertia(rectangle_mass);
220 let circle_inertia = circle.angular_inertia(circle_mass);
221
222 let mut capsule_inertia = rectangle_inertia + circle_inertia;
224
225 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 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 let rectangle_area = rectangle.area();
249 let circle_area = circle.area();
250
251 let rectangle_mass = rectangle_area * density;
253 let circle_mass = circle_area * density;
254
255 let rectangle_inertia = rectangle.angular_inertia(rectangle_mass);
257 let circle_inertia = circle.angular_inertia(circle_mass);
258
259 let mut capsule_inertia = rectangle_inertia + circle_inertia;
261
262 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 let mut area = 0.0;
280
281 let mut iter = vertices.iter().peekable();
283 let Some(first) = iter.peek().copied().copied() else {
284 return 0.0;
285 };
286
287 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 let mut inertia = 0.0;
314
315 let mut iter = vertices.iter().peekable();
317 let first = **iter.peek().unwrap();
318
319 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 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 let mut inertia = 0.0;
351
352 let mut iter = vertices.iter().peekable();
354 let first = **iter.peek().unwrap();
355
356 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 let mut area = 0.0;
378 let mut center = Vec2::ZERO;
379
380 let mut iter = vertices.iter().peekable();
382 let Some(first) = iter.peek().copied().copied() else {
383 return (0.0, Vec2::ZERO);
384 };
385
386 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 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 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 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 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 test_shape!(circle, Circle::new(2.0));
496 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!(capsule, Capsule2d::new(1.0, 0.25));
513}