rhusics_core/physics/
volumes.rs

1use cgmath::{
2    BaseFloat, EuclideanSpace, InnerSpace, Matrix3, Point2, Point3, SquareMatrix, Transform,
3    Vector3, Zero,
4};
5use collision::primitive::*;
6use collision::{Aabb, Aabb2, Aabb3, Bound, ComputeBound, Primitive, Union};
7
8use super::{Inertia, Mass, Material, PartialCrossProduct};
9use collide::CollisionShape;
10
11/// Describe a shape with volume
12///
13/// ### Type parameters:
14///
15/// - `I`: Inertia type, see `Inertia` for more information
16pub trait Volume<S, I> {
17    /// Compute the mass of the shape based on its material
18    fn get_mass(&self, material: &Material) -> Mass<S, I>;
19}
20
21impl<S> Volume<S, S> for Circle<S>
22where
23    S: BaseFloat + Inertia,
24{
25    fn get_mass(&self, material: &Material) -> Mass<S, S> {
26        use std::f64::consts::PI;
27        let pi = S::from(PI).unwrap();
28        let mass = pi * self.radius * self.radius * material.density();
29        let inertia = mass * self.radius * self.radius / (S::one() + S::one());
30        Mass::new_with_inertia(mass, inertia)
31    }
32}
33
34impl<S> Volume<S, S> for Rectangle<S>
35where
36    S: BaseFloat + Inertia,
37{
38    fn get_mass(&self, material: &Material) -> Mass<S, S> {
39        let b: Aabb2<S> = self.compute_bound();
40        let mass = b.volume() * material.density();
41        let inertia =
42            mass * (b.dim().x * b.dim().x + b.dim().y * b.dim().y) / S::from(12.).unwrap();
43        Mass::new_with_inertia(mass, inertia)
44    }
45}
46
47impl<S> Volume<S, S> for Square<S>
48where
49    S: BaseFloat + Inertia,
50{
51    fn get_mass(&self, material: &Material) -> Mass<S, S> {
52        let b: Aabb2<S> = self.compute_bound();
53        let mass = b.volume() * material.density();
54        let inertia =
55            mass * (b.dim().x * b.dim().x + b.dim().y * b.dim().y) / S::from(12.).unwrap();
56        Mass::new_with_inertia(mass, inertia)
57    }
58}
59
60impl<S> Volume<S, S> for ConvexPolygon<S>
61where
62    S: BaseFloat + Inertia,
63{
64    fn get_mass(&self, material: &Material) -> Mass<S, S> {
65        let mut area = S::zero();
66        let mut denom = S::zero();
67        for i in 0..self.vertices.len() {
68            let j = if i == self.vertices.len() - 1 {
69                0
70            } else {
71                i + 1
72            };
73            let p0 = self.vertices[i].to_vec();
74            let p1 = self.vertices[j].to_vec();
75            let a = p0.cross(&p1).abs();
76            let b = p0.dot(p0) + p0.dot(p1) + p1.dot(p1);
77            denom += a * b;
78            area += a;
79        }
80        let mass = area * S::from(0.5).unwrap() * material.density();
81        let inertia = mass / S::from(6.).unwrap() * denom / area;
82        Mass::new_with_inertia(mass, inertia)
83    }
84}
85
86impl<S> Volume<S, Matrix3<S>> for Sphere<S>
87where
88    S: BaseFloat,
89{
90    fn get_mass(&self, material: &Material) -> Mass<S, Matrix3<S>> {
91        use std::f64::consts::PI;
92        let pi = S::from(PI).unwrap();
93        let mass = S::from(4. / 3.).unwrap()
94            * pi
95            * self.radius
96            * self.radius
97            * self.radius
98            * material.density();
99        let inertia = S::from(2. / 5.).unwrap() * mass * self.radius * self.radius;
100        Mass::new_with_inertia(mass, Matrix3::from_value(inertia))
101    }
102}
103
104impl<S> Volume<S, Matrix3<S>> for Cuboid<S>
105where
106    S: BaseFloat,
107{
108    fn get_mass(&self, material: &Material) -> Mass<S, Matrix3<S>> {
109        let b: Aabb3<S> = self.compute_bound();
110        let mass = b.volume() * material.density();
111        let x2 = b.dim().x * b.dim().x;
112        let y2 = b.dim().y * b.dim().y;
113        let z2 = b.dim().z * b.dim().z;
114        let mnorm = mass / S::from(12.).unwrap();
115        let inertia = Matrix3::from_diagonal(Vector3::new(y2 + z2, x2 + z2, x2 + y2) * mnorm);
116        Mass::new_with_inertia(mass, inertia)
117    }
118}
119
120impl<S> Volume<S, Matrix3<S>> for Cube<S>
121where
122    S: BaseFloat,
123{
124    fn get_mass(&self, material: &Material) -> Mass<S, Matrix3<S>> {
125        let b: Aabb3<S> = self.compute_bound();
126        let mass = b.volume() * material.density();
127        let x2 = b.dim().x * b.dim().x;
128        let y2 = b.dim().y * b.dim().y;
129        let z2 = b.dim().z * b.dim().z;
130        let mnorm = mass / S::from(12.).unwrap();
131        let inertia = Matrix3::from_diagonal(Vector3::new(y2 + z2, x2 + z2, x2 + y2) * mnorm);
132        Mass::new_with_inertia(mass, inertia)
133    }
134}
135
136fn poly_sub_expr_calc<S>(w0: S, w1: S, w2: S) -> (S, S, S, S, S, S)
137where
138    S: BaseFloat,
139{
140    let t0 = w0 + w1;
141    let t1 = w0 * w0;
142    let t2 = t1 + t0 * w1;
143    let f1 = t0 + w2;
144    let f2 = t2 + w2 * f1;
145    let f3 = w0 * t0 + w1 * t2 + w2 * f2;
146    (
147        f1,
148        f2,
149        f3,
150        f2 + w0 * (f1 + w0),
151        f2 + w1 * (f1 + w1),
152        f2 + w2 * (f1 + w2),
153    )
154}
155
156const ONE_6: f64 = 1. / 6.;
157const ONE_24: f64 = 1. / 24.;
158const ONE_60: f64 = 1. / 60.;
159const ONE_120: f64 = 1. / 120.;
160const POLY_SCALE: [f64; 10] = [
161    ONE_6, ONE_24, ONE_24, ONE_24, ONE_60, ONE_60, ONE_60, ONE_120, ONE_120, ONE_120,
162];
163
164impl<S> Volume<S, Matrix3<S>> for ConvexPolyhedron<S>
165where
166    S: BaseFloat,
167{
168    // Volume of tetrahedron is 1/6 * a.cross(b).dot(c) where a = B - C, b = A - C, c = Origin - C
169    // Sum for all faces
170    fn get_mass(&self, material: &Material) -> Mass<S, Matrix3<S>> {
171        let mut intg: [S; 10] = [S::zero(); 10];
172        for (p0, p1, p2) in self.faces_iter() {
173            let v1 = p1 - p0; // a1, b1, c1
174            let v2 = p2 - p0; // a2, b2, c2
175            let d = v1.cross(v2); // d0, d1, d2
176            let (f1x, f2x, f3x, g0x, g1x, g2x) = poly_sub_expr_calc(p0.x, p1.x, p2.x);
177            let (_, f2y, f3y, g0y, g1y, g2y) = poly_sub_expr_calc(p0.y, p1.y, p2.y);
178            let (_, f2z, f3z, g0z, g1z, g2z) = poly_sub_expr_calc(p0.z, p1.z, p2.z);
179            intg[0] += d.x * f1x;
180            intg[1] += d.x * f2x;
181            intg[2] += d.y * f2y;
182            intg[3] += d.z * f2z;
183            intg[4] += d.x * f3x;
184            intg[5] += d.y * f3y;
185            intg[6] += d.z * f3z;
186            intg[7] += d.x * (p0.y * g0x + p1.y * g1x + p2.y * g2x);
187            intg[8] += d.y * (p0.z * g0y + p1.z * g1y + p2.z * g2y);
188            intg[9] += d.z * (p0.x * g0z + p1.x * g1z + p2.x * g2z);
189        }
190        for i in 0..10 {
191            intg[i] *= S::from(POLY_SCALE[i]).unwrap();
192        }
193        let cm = Point3::new(intg[1] / intg[0], intg[2] / intg[0], intg[3] / intg[0]);
194        let mut inertia = Matrix3::zero();
195        inertia.x.x = intg[5] + intg[6] - intg[0] * (cm.y * cm.y + cm.z * cm.z);
196        inertia.y.y = intg[4] + intg[6] - intg[0] * (cm.x * cm.x + cm.z * cm.z);
197        inertia.z.z = intg[4] + intg[5] - intg[0] * (cm.x * cm.x + cm.y * cm.y);
198        inertia.x.y = -(intg[7] - intg[0] * cm.x * cm.y);
199        inertia.y.z = -(intg[8] - intg[0] * cm.y * cm.z);
200        inertia.x.z = -(intg[9] - intg[0] * cm.x * cm.z);
201        Mass::new_with_inertia(
202            intg[0] * material.density(),
203            inertia * material.density::<S>(),
204        )
205    }
206}
207
208impl<S> Volume<S, S> for Primitive2<S>
209where
210    S: BaseFloat + Inertia,
211{
212    fn get_mass(&self, material: &Material) -> Mass<S, S> {
213        use collision::primitive::Primitive2::*;
214        match *self {
215            Particle(_) | Line(_) => Mass::new(material.density()),
216            Circle(ref circle) => circle.get_mass(material),
217            Rectangle(ref rectangle) => rectangle.get_mass(material),
218            Square(ref square) => square.get_mass(material),
219            ConvexPolygon(ref polygon) => polygon.get_mass(material),
220        }
221    }
222}
223
224impl<S> Volume<S, Matrix3<S>> for Capsule<S>
225where
226    S: BaseFloat,
227{
228    fn get_mass(&self, material: &Material) -> Mass<S, Matrix3<S>> {
229        use std::f64::consts::PI;
230        let pi = S::from(PI).unwrap();
231        let rsq = self.radius() * self.radius();
232        let hsq = self.height() * self.height();
233        let two = S::one() + S::one();
234        let three = two + S::one();
235        let four = two + two;
236        let five = three + two;
237        let eight = five + three;
238        let twelve = eight + four;
239        let c_m = pi * rsq * self.height() * material.density();
240        let h_m = pi * two / three * rsq * self.radius() * material.density();
241        let mass = c_m + two * h_m;
242        let c_i_xz = hsq / twelve + rsq / four;
243        let h_i_xz = rsq * two / five + hsq / two + self.height() * self.radius() * three / eight;
244        let i_xz = c_m * c_i_xz + h_m * h_i_xz * two;
245        let i_y = c_m * rsq / two + h_m * rsq * four / five;
246        let inertia = Matrix3::from_diagonal(Vector3::new(i_xz, i_y, i_xz));
247        Mass::new_with_inertia(mass, inertia)
248    }
249}
250
251impl<S> Volume<S, Matrix3<S>> for Cylinder<S>
252where
253    S: BaseFloat,
254{
255    fn get_mass(&self, material: &Material) -> Mass<S, Matrix3<S>> {
256        use std::f64::consts::PI;
257        let pi = S::from(PI).unwrap();
258        let rsq = self.radius() * self.radius();
259        let volume = pi * rsq * self.height();
260        let mass = volume * material.density();
261        let two = S::one() + S::one();
262        let three = S::one() + two;
263        let twelve = three * two * two;
264        let i_y = mass * rsq / two;
265        let i_xz = mass / twelve * (three * rsq + self.height() * self.height());
266        let inertia = Matrix3::from_diagonal(Vector3::new(i_xz, i_y, i_xz));
267        Mass::new_with_inertia(mass, inertia)
268    }
269}
270
271impl<S> Volume<S, Matrix3<S>> for Primitive3<S>
272where
273    S: BaseFloat,
274{
275    fn get_mass(&self, material: &Material) -> Mass<S, Matrix3<S>> {
276        use collision::primitive::Primitive3::*;
277        match *self {
278            Particle(_) | Quad(_) => Mass::new(material.density()),
279            Sphere(ref sphere) => sphere.get_mass(material),
280            Cuboid(ref cuboid) => cuboid.get_mass(material),
281            Cube(ref cube) => cube.get_mass(material),
282            Capsule(ref capsule) => capsule.get_mass(material),
283            Cylinder(ref cylinder) => cylinder.get_mass(material),
284            ConvexPolyhedron(ref polyhedra) => polyhedra.get_mass(material),
285        }
286    }
287}
288
289// Composite inertia : sum(I_i + M_i * d_i^2)
290// I_i : Inertia of primitive with index i
291// M_i : Mass of primitive with index i
292// d_i : Offset from composite center of mass to primitive center of mass
293impl<S, P, T, B, Y> Volume<S, S> for CollisionShape<P, T, B, Y>
294where
295    S: BaseFloat + Inertia,
296    P: Volume<S, S> + Primitive<Point = Point2<S>> + ComputeBound<B>,
297    B: Bound<Point = Point2<S>> + Clone + Union<B, Output = B>,
298    T: Transform<Point2<S>>,
299    Y: Default,
300{
301    fn get_mass(&self, material: &Material) -> Mass<S, S> {
302        let (mass, inertia) = self
303            .primitives()
304            .iter()
305            .map(|p| (p.0.get_mass(material), &p.1))
306            .fold((S::zero(), S::zero()), |(a_m, a_i), (m, t)| {
307                (a_m + m.mass(), a_i + m.local_inertia() + m.mass() * d2(t))
308            });
309        Mass::new_with_inertia(mass, inertia)
310    }
311}
312
313fn d2<S, T>(t: &T) -> S
314where
315    S: BaseFloat,
316    T: Transform<Point2<S>>,
317{
318    let p = t.transform_point(Point2::origin()).to_vec();
319    p.dot(p)
320}
321
322impl<S, P, T, B, Y> Volume<S, Matrix3<S>> for CollisionShape<P, T, B, Y>
323where
324    S: BaseFloat,
325    P: Volume<S, Matrix3<S>> + Primitive<Point = Point3<S>> + ComputeBound<B>,
326    B: Bound<Point = Point3<S>> + Clone + Union<B, Output = B>,
327    T: Transform<Point3<S>>,
328    Y: Default,
329{
330    fn get_mass(&self, material: &Material) -> Mass<S, Matrix3<S>> {
331        let (mass, inertia) = self
332            .primitives()
333            .iter()
334            .map(|p| (p.0.get_mass(material), &p.1))
335            .fold((S::zero(), Matrix3::zero()), |(a_m, a_i), (m, t)| {
336                (a_m + m.mass(), a_i + m.local_inertia() + d3(t) * m.mass())
337            });
338        Mass::new_with_inertia(mass, inertia)
339    }
340}
341
342fn d3<S, T>(t: &T) -> Matrix3<S>
343where
344    S: BaseFloat,
345    T: Transform<Point3<S>>,
346{
347    let o = t.transform_point(Point3::origin()).to_vec();
348    let d2 = o.magnitude2();
349    let mut j = Matrix3::from_value(d2);
350    j.x += o * -o.x;
351    j.y += o * -o.y;
352    j.z += o * -o.z;
353    j
354}
rhusics_core/physics/volumes.rs

rhusics_core/physics/
volumes.rs
