1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
|
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)
}
|
