nphysics2d/volumetric/
volumetric_convex2.rs

1use num::Zero;
2
3use crate::math::{AngularInertia, Point};
4use crate::volumetric::Volumetric;
5use na;
6use na::RealField;
7use na::{Matrix1, Point2};
8use ncollide::shape::ConvexPolygon;
9use ncollide::utils;
10
11/// The area and center of mass of a 2D convex Polyline.
12///
13/// The polyline is not checked to be actually convex.
14pub fn convex_polyline_area_and_center_of_mass_unchecked<N: RealField + Copy>(
15    convex_polyline: &[Point<N>],
16) -> (N, Point<N>) {
17    let geometric_center = utils::center(convex_polyline);
18    let mut res = Point::origin();
19    let mut areasum = N::zero();
20
21    let mut iterpeek = convex_polyline.iter().peekable();
22    let firstelement = *iterpeek.peek().unwrap(); // Stores first element to close the cycle in the end with unwrap_or.
23    while let Some(elem) = iterpeek.next() {
24        let area = utils::triangle_area(
25            elem,
26            iterpeek.peek().unwrap_or(&firstelement),
27            &geometric_center,
28        );
29        let center = utils::triangle_center(
30            elem,
31            iterpeek.peek().unwrap_or(&firstelement),
32            &geometric_center,
33        );
34
35        res += center.coords * area;
36        areasum += area;
37    }
38
39    if areasum.is_zero() {
40        (areasum, geometric_center)
41    } else {
42        (areasum, res / areasum)
43    }
44}
45
46/// The mass properties of a 2D convex Polyline.
47///
48/// The polyline is not checked to be actually convex.
49pub fn convex_polyline_mass_properties_unchecked<N: RealField + Copy>(
50    convex_polyline: &[Point<N>],
51    density: N,
52) -> (N, Point<N>, N) {
53    let (area, com) = convex_polyline_area_and_center_of_mass_unchecked(convex_polyline);
54
55    if area.is_zero() {
56        return (na::zero(), com, na::zero());
57    }
58
59    let mut itot = N::zero();
60    let factor: N = na::convert(0.5 * 1.0 / 3.0);
61
62    let mut iterpeek = convex_polyline.iter().peekable();
63    let firstelement = *iterpeek.peek().unwrap(); // store first element to close the cycle in the end with unwrap_or
64    while let Some(elem) = iterpeek.next() {
65        let area = utils::triangle_area(&com, elem, iterpeek.peek().unwrap_or(&firstelement));
66
67        // algorithm adapted from Box2D
68        let e1 = *elem - com;
69        let e2 = **(iterpeek.peek().unwrap_or(&firstelement)) - com;
70
71        let ex1 = e1[0];
72        let ey1 = e1[1];
73        let ex2 = e2[0];
74        let ey2 = e2[1];
75
76        let intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2;
77        let inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2;
78
79        let ipart = factor * (intx2 + inty2);
80
81        itot += ipart * area;
82    }
83
84    (area * density, com, itot * density)
85}
86
87/// The area of a convex polyline.
88///
89/// The polyline is not checked to be actually convex.
90pub fn convex_polyline_area_unchecked<N: RealField + Copy>(convex_polyline: &[Point<N>]) -> N {
91    let geometric_center = utils::center(convex_polyline);
92    let mut areasum = N::zero();
93
94    let mut iterpeek = convex_polyline.iter().peekable();
95    let firstelement = *iterpeek.peek().unwrap(); // Store first element to close the cycle in the end with unwrap_or.
96    while let Some(elem) = iterpeek.next() {
97        let area = utils::triangle_area(
98            elem,
99            iterpeek.peek().unwrap_or(&firstelement),
100            &geometric_center,
101        );
102
103        areasum += area;
104    }
105
106    areasum
107}
108
109/// The area of a convex hull.
110pub fn convex_hull_area<N: RealField + Copy>(points: &[Point<N>]) -> N {
111    convex_polyline_area_unchecked(points)
112}
113
114/// The volume of the convex hull of a set of points.
115pub fn convex_hull_volume<N: RealField + Copy>(points: &[Point<N>]) -> N {
116    convex_hull_area(points)
117}
118
119/// The center of mass of the convex hull of a set of points.
120pub fn convex_hull_center_of_mass<N: RealField + Copy>(points: &[Point<N>]) -> Point<N> {
121    convex_polyline_area_and_center_of_mass_unchecked(points).1
122}
123
124/// The angular inertia of the convex hull of a set of points.
125pub fn convex_hull_unit_angular_inertia<N: RealField + Copy>(points: &[Point<N>]) -> AngularInertia<N> {
126    let (area, _, i): (_, _, N) = convex_polyline_mass_properties_unchecked(points, na::one());
127
128    let mut tensor = AngularInertia::zero();
129    tensor[(0, 0)] = i * (N::one() / area);
130
131    tensor
132}
133
134impl<N: RealField + Copy> Volumetric<N> for ConvexPolygon<N> {
135    fn area(&self) -> N {
136        convex_hull_area(self.points())
137    }
138
139    fn volume(&self) -> N {
140        convex_hull_volume(self.points())
141    }
142
143    fn center_of_mass(&self) -> Point2<N> {
144        convex_hull_center_of_mass(self.points())
145    }
146
147    fn unit_angular_inertia(&self) -> Matrix1<N> {
148        convex_hull_unit_angular_inertia(self.points())
149    }
150
151    fn mass_properties(&self, density: N) -> (N, Point2<N>, Matrix1<N>) {
152        let (r1, r2, r3) = convex_polyline_mass_properties_unchecked(self.points(), density);
153        (r1, r2, Matrix1::<N>::new(r3))
154    }
155}
156
157#[cfg(test)]
158mod test {
159    use na::{self, Matrix1, Point2, Vector2};
160    use ncollide::shape::{ConvexPolygon, Cuboid};
161
162    use crate::volumetric::Volumetric;
163
164    #[test]
165    fn test_inertia_tensor2() {
166        // square
167        let a = 3.8f32; // some value for side length
168        let half_a = a / 2.0;
169
170        // real moment of inertia but divided by the area of the square
171        let real_moi = a.powf(2.0) / 6.0;
172        let expected = Matrix1::new(real_moi);
173
174        // standard cuboid
175        let cube = Cuboid::new(Vector2::new(half_a, half_a));
176
177        let actual = cube.unit_angular_inertia();
178        assert!(
179            relative_eq!(actual, expected),
180            format!(
181                "Inertia values do not match: actual {:?}, expected: {:?}.",
182                actual, expected
183            )
184        );
185
186        // convex shape
187        let geom = {
188            let points = vec![
189                Point2::new(half_a, half_a),
190                Point2::new(-half_a, half_a),
191                Point2::new(-half_a, -half_a),
192                Point2::new(half_a, -half_a),
193            ];
194            ConvexPolygon::try_new(points).unwrap()
195        };
196        let actual = geom.unit_angular_inertia();
197        assert!(
198            relative_eq!(actual, expected),
199            format!(
200                "Inertia values do not match: actual {:?}, expected: {:?}.",
201                actual, expected
202            )
203        );
204
205        // rectangle
206        let a = 2.3f32; // some values for side lengths
207        let b = 6.7f32;
208        let half_a = a / 2.0;
209        let half_b = b / 2.0;
210
211        // real moment of inertia but divided by the area of the rectangle
212        let real_moi = (1.0 / 12.0) * (a.powf(2.0) + b.powf(2.0));
213        let expected = Matrix1::new(real_moi);
214
215        // standard cuboid
216        let cube = Cuboid::new(Vector2::new(half_a, half_b));
217
218        let actual = cube.unit_angular_inertia();
219        assert!(
220            relative_eq!(actual, expected),
221            format!(
222                "Inertia values do not match: actual {:?}, expected: {:?}.",
223                actual, expected
224            )
225        );
226
227        // convex shape
228        let geom = {
229            let points = vec![
230                Point2::new(half_a, half_b),
231                Point2::new(-half_a, half_b),
232                Point2::new(-half_a, -half_b),
233                Point2::new(half_a, -half_b),
234            ];
235            ConvexPolygon::try_new(points).unwrap()
236        };
237        let actual = geom.unit_angular_inertia();
238        assert!(
239            relative_eq!(actual, expected),
240            format!(
241                "Inertia values do not match: actual {:?}, expected: {:?}.",
242                actual, expected
243            )
244        );
245
246        // triangle
247        let b = 6.7f32; // base length
248        let h = 3.8f32; // height
249        let a = 2.3f32; // offset of triangle top to base begin on the x-axis
250                        // if the base line is on the x-axis and starts at 0, the the top of the triangle
251                        // is at x = a, y = h
252
253        let c_x = a + b / 3.0;
254        let c_y = h / 3.0;
255
256        // real moment of inertia but divided by the area of the triangle
257        // formula taken from http://www.efunda.com/math/areas/triangle.cfm
258        let area = b * h / 2.0;
259        let real_moi =
260            (b.powf(3.0) * h - b.powf(2.0) * h * a + b * h * a.powf(2.0) + b * h.powf(3.0))
261                / (36.0 * area);
262        let expected = Matrix1::new(real_moi);
263
264        // convex shape
265        let geom = {
266            let points = vec![
267                Point2::new(0.0 - c_x, 0.0 - c_y),
268                Point2::new(b - c_x, 0.0 - c_y),
269                Point2::new(a - c_x, h - c_y),
270            ];
271            ConvexPolygon::try_new(points).unwrap()
272        };
273        let actual = geom.unit_angular_inertia();
274        assert!(
275            relative_eq!(actual, expected),
276            format!(
277                "Inertia values do not match: actual {:?}, expected: {:?}.",
278                actual, expected
279            )
280        );
281    }
282}
nphysics2d/volumetric/volumetric_convex2.rs

nphysics2d/volumetric/
volumetric_convex2.rs
