aboutsummaryrefslogtreecommitdiff
path: root/src/dynamics/mass_properties_polygon.rs
diff options
context:
space:
mode:
Diffstat (limited to 'src/dynamics/mass_properties_polygon.rs')
-rw-r--r--src/dynamics/mass_properties_polygon.rs146
1 files changed, 0 insertions, 146 deletions
diff --git a/src/dynamics/mass_properties_polygon.rs b/src/dynamics/mass_properties_polygon.rs
deleted file mode 100644
index 8b0b811..0000000
--- a/src/dynamics/mass_properties_polygon.rs
+++ /dev/null
@@ -1,146 +0,0 @@
-#![allow(dead_code)] // TODO: remove this
-
-use crate::dynamics::MassProperties;
-use crate::math::Point;
-
-impl MassProperties {
- pub(crate) fn from_polygon(density: f32, vertices: &[Point<f32>]) -> MassProperties {
- let (area, com) = convex_polygon_area_and_center_of_mass(vertices);
-
- if area == 0.0 {
- return MassProperties::new(com, 0.0, 0.0);
- }
-
- let mut itot = 0.0;
- let factor = 1.0 / 6.0;
-
- let mut iterpeek = vertices.iter().peekable();
- let firstelement = *iterpeek.peek().unwrap(); // store first element to close the cycle in the end with unwrap_or
- while let Some(elem) = iterpeek.next() {
- let area = triangle_area(&com, elem, iterpeek.peek().unwrap_or(&firstelement));
-
- // algorithm adapted from Box2D
- let e1 = *elem - com;
- let e2 = **(iterpeek.peek().unwrap_or(&firstelement)) - com;
-
- let ex1 = e1[0];
- let ey1 = e1[1];
- let ex2 = e2[0];
- let ey2 = e2[1];
-
- let intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2;
- let inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2;
-
- let ipart = factor * (intx2 + inty2);
-
- itot += ipart * area;
- }
-
- Self::new(com, area * density, itot * density)
- }
-}
-
-fn convex_polygon_area_and_center_of_mass(convex_polygon: &[Point<f32>]) -> (f32, Point<f32>) {
- let geometric_center = convex_polygon
- .iter()
- .fold(Point::origin(), |e1, e2| e1 + e2.coords)
- / convex_polygon.len() as f32;
- let mut res = Point::origin();
- let mut areasum = 0.0;
-
- let mut iterpeek = convex_polygon.iter().peekable();
- let firstelement = *iterpeek.peek().unwrap(); // Stores first element to close the cycle in the end with unwrap_or.
- while let Some(elem) = iterpeek.next() {
- let (a, b, c) = (
- elem,
- iterpeek.peek().unwrap_or(&firstelement),
- &geometric_center,
- );
- let area = triangle_area(a, b, c);
- let center = (a.coords + b.coords + c.coords) / 3.0;
-
- res += center * area;
- areasum += area;
- }
-
- if areasum == 0.0 {
- (areasum, geometric_center)
- } else {
- (areasum, res / areasum)
- }
-}
-
-pub fn triangle_area(pa: &Point<f32>, pb: &Point<f32>, pc: &Point<f32>) -> f32 {
- // Kahan's formula.
- let a = na::distance(pa, pb);
- let b = na::distance(pb, pc);
- let c = na::distance(pc, pa);
-
- let (c, b, a) = sort3(&a, &b, &c);
- let a = *a;
- let b = *b;
- let c = *c;
-
- let sqr = (a + (b + c)) * (c - (a - b)) * (c + (a - b)) * (a + (b - c));
-
- sqr.sqrt() * 0.25
-}
-
-/// Sorts a set of three values in increasing order.
-#[inline]
-pub fn sort3<'a>(a: &'a f32, b: &'a f32, c: &'a f32) -> (&'a f32, &'a f32, &'a f32) {
- let a_b = *a > *b;
- let a_c = *a > *c;
- let b_c = *b > *c;
-
- let sa;
- let sb;
- let sc;
-
- // Sort the three values.
- // FIXME: move this to the utilities?
- if a_b {
- // a > b
- if a_c {
- // a > c
- sc = a;
-
- if b_c {
- // b > c
- sa = c;
- sb = b;
- } else {
- // b <= c
- sa = b;
- sb = c;
- }
- } else {
- // a <= c
- sa = b;
- sb = a;
- sc = c;
- }
- } else {
- // a < b
- if !a_c {
- // a <= c
- sa = a;
-
- if b_c {
- // b > c
- sb = c;
- sc = b;
- } else {
- sb = b;
- sc = c;
- }
- } else {
- // a > c
- sa = c;
- sb = a;
- sc = b;
- }
- }
-
- (sa, sb, sc)
-}