1use crate::collider::ColliderDesc;
2use crate::shape::{Shape, SolidGeometry};
3
4pub struct MassProperties {
5 pub com: [f32; 3],
6 pub inertia: [f32; 6],
7 pub inverse_inertia: [f32; 6],
8}
9
10impl MassProperties {
11 pub fn zeroed() -> Self {
12 Self {
13 com: [0.0; 3],
14 inertia: [0.0; 6],
15 inverse_inertia: [0.0; 6],
16 }
17 }
18
19 fn of(com: [f32; 3], inertia: [f32; 6]) -> Self {
20 Self {
21 com,
22 inverse_inertia: inertia_inverse(inertia),
23 inertia,
24 }
25 }
26}
27
28#[derive(Clone, Copy)]
29pub enum MassSource {
30 Fixed(f32),
31 Density(f32),
32}
33
34pub fn analytic_solid(shape: &Shape) -> Option<SolidGeometry> {
35 Some(match *shape {
36 Shape::Sphere { radius } => {
37 let i = 2.0 / 5.0 * radius * radius;
38 SolidGeometry {
39 volume: 4.0 / 3.0 * std::f32::consts::PI * radius * radius * radius,
40 centroid: [0.0; 3],
41 unit_inertia: [i, 0.0, 0.0, i, 0.0, i],
42 }
43 }
44 Shape::Cuboid { half_extents } => {
45 let ex = half_extents[0] * 2.0;
46 let ey = half_extents[1] * 2.0;
47 let ez = half_extents[2] * 2.0;
48 SolidGeometry {
49 volume: ex * ey * ez,
50 centroid: [0.0; 3],
51 unit_inertia: [
52 (ey * ey + ez * ez) / 12.0,
53 0.0,
54 0.0,
55 (ex * ex + ez * ez) / 12.0,
56 0.0,
57 (ex * ex + ey * ey) / 12.0,
58 ],
59 }
60 }
61 Shape::Capsule {
62 radius,
63 half_height,
64 } => {
65 let cylinder = std::f32::consts::PI * radius * radius * 2.0 * half_height;
66 let sphere = 4.0 / 3.0 * std::f32::consts::PI * radius * radius * radius;
67 let total = cylinder + sphere;
68 let height = half_height + 3.0 / 8.0 * radius;
69 let axial = (cylinder * 0.5 * radius * radius + 0.4 * sphere * radius * radius) / total;
70 let lateral = (cylinder / 12.0
71 * (3.0 * radius * radius + 4.0 * half_height * half_height)
72 + sphere * (83.0 / 320.0 * radius * radius + height * height))
73 / total;
74 SolidGeometry {
75 volume: total,
76 centroid: [0.0; 3],
77 unit_inertia: [lateral, 0.0, 0.0, axial, 0.0, lateral],
78 }
79 }
80 Shape::Cylinder {
81 radius,
82 half_height,
83 } => {
84 let height = half_height * 2.0;
85 let lateral = (3.0 * radius * radius + height * height) / 12.0;
86 SolidGeometry {
87 volume: std::f32::consts::PI * radius * radius * height,
88 centroid: [0.0; 3],
89 unit_inertia: [lateral, 0.0, 0.0, 0.5 * radius * radius, 0.0, lateral],
90 }
91 }
92 Shape::Hull(_) | Shape::Mesh(_) | Shape::HeightField(_) | Shape::Plane => return None,
93 })
94}
95
96pub fn shape_solid(
97 shape: &Shape,
98 source: impl Fn(&Shape) -> Option<SolidGeometry>,
99) -> Option<SolidGeometry> {
100 match shape {
101 Shape::Hull(_) => {
102 Some(source(shape).expect("a hull shape source must answer its solid geometry"))
103 }
104 Shape::Mesh(_) | Shape::HeightField(_) | Shape::Plane => None,
105 _ => analytic_solid(shape),
106 }
107}
108
109fn collider_solid(
110 collider: &ColliderDesc,
111 source: &impl Fn(&Shape) -> Option<SolidGeometry>,
112) -> Option<SolidGeometry> {
113 if collider.sensor {
114 return None;
115 }
116 shape_solid(&collider.shape, source)
117}
118
119fn scale_volume(scale: [f32; 3]) -> f32 {
120 scale[0] * scale[1] * scale[2]
121}
122
123fn rotate(q: [f32; 4], v: [f32; 3]) -> [f32; 3] {
124 crate::math::quat_rotate(q, v)
125}
126
127fn centroid_of(collider: &ColliderDesc, solid: &SolidGeometry) -> [f32; 3] {
128 let scaled = [
129 collider.scale[0] * solid.centroid[0],
130 collider.scale[1] * solid.centroid[1],
131 collider.scale[2] * solid.centroid[2],
132 ];
133 let rotated = rotate(collider.rotation, scaled);
134 [
135 collider.offset[0] + rotated[0],
136 collider.offset[1] + rotated[1],
137 collider.offset[2] + rotated[2],
138 ]
139}
140
141pub fn mass_properties_of_intent(
142 colliders: &[ColliderDesc],
143 mass: f32,
144 com: Option<[f32; 3]>,
145 inertia: Option<[f32; 6]>,
146 source: impl Fn(&Shape) -> Option<SolidGeometry>,
147) -> MassProperties {
148 if let Some(inertia) = inertia {
149 return MassProperties::of(com.unwrap_or([0.0; 3]), inertia);
150 }
151 compute_mass_properties(colliders, MassSource::Fixed(mass), com, source)
152}
153
154pub fn compute_mass_properties(
155 colliders: &[ColliderDesc],
156 source: MassSource,
157 com: Option<[f32; 3]>,
158 geometry: impl Fn(&Shape) -> Option<SolidGeometry>,
159) -> MassProperties {
160 let solids = colliders
161 .iter()
162 .filter_map(|collider| collider_solid(collider, &geometry).map(|solid| (collider, solid)))
163 .collect::<Vec<_>>();
164 if solids.is_empty() {
165 return MassProperties::zeroed();
166 }
167 let volumes = solids
168 .iter()
169 .map(|(collider, solid)| scale_volume(collider.scale) * solid.volume)
170 .collect::<Vec<_>>();
171 let total_volume = volumes.iter().sum::<f32>();
172 if total_volume <= 0.0 {
173 return MassProperties::zeroed();
174 }
175 let mass = match source {
176 MassSource::Fixed(mass) => mass,
177 MassSource::Density(density) => density * total_volume,
178 };
179 if mass <= 0.0 {
180 return MassProperties::zeroed();
181 }
182 let com = com.unwrap_or_else(|| {
183 let mut sum = [0.0f32; 3];
184 for ((collider, solid), volume) in solids.iter().zip(&volumes) {
185 let at = centroid_of(collider, solid);
186 sum[0] += at[0] * volume;
187 sum[1] += at[1] * volume;
188 sum[2] += at[2] * volume;
189 }
190 [
191 sum[0] / total_volume,
192 sum[1] / total_volume,
193 sum[2] / total_volume,
194 ]
195 });
196 let mut inertia = [0.0f32; 6];
197 for ((collider, solid), volume) in solids.iter().zip(&volumes) {
198 let shape_mass = mass * volume / total_volume;
199 let local = inertia_scale(solid.unit_inertia, collider.scale);
200 let scaled = [
201 local[0] * shape_mass,
202 local[1] * shape_mass,
203 local[2] * shape_mass,
204 local[3] * shape_mass,
205 local[4] * shape_mass,
206 local[5] * shape_mass,
207 ];
208 let rotated = inertia_rotate(scaled, collider.rotation);
209 let at = centroid_of(collider, solid);
210 let offset = [at[0] - com[0], at[1] - com[1], at[2] - com[2]];
211 let translated = inertia_translate(rotated, offset, shape_mass);
212 inertia[0] += translated[0];
213 inertia[1] += translated[1];
214 inertia[2] += translated[2];
215 inertia[3] += translated[3];
216 inertia[4] += translated[4];
217 inertia[5] += translated[5];
218 }
219 MassProperties::of(com, inertia)
220}
221
222pub fn solid_volume_of(
223 colliders: &[ColliderDesc],
224 geometry: impl Fn(&Shape) -> Option<SolidGeometry>,
225) -> f32 {
226 colliders
227 .iter()
228 .filter_map(|collider| {
229 collider_solid(collider, &geometry)
230 .map(|solid| scale_volume(collider.scale) * solid.volume)
231 })
232 .sum()
233}
234
235fn inertia_translate(inertia: [f32; 6], offset: [f32; 3], mass: f32) -> [f32; 6] {
236 let d = offset;
237 [
238 inertia[0] + mass * (d[1] * d[1] + d[2] * d[2]),
239 inertia[1] - mass * d[0] * d[1],
240 inertia[2] - mass * d[0] * d[2],
241 inertia[3] + mass * (d[0] * d[0] + d[2] * d[2]),
242 inertia[4] - mass * d[1] * d[2],
243 inertia[5] + mass * (d[0] * d[0] + d[1] * d[1]),
244 ]
245}
246
247fn inertia_scale(inertia: [f32; 6], scale: [f32; 3]) -> [f32; 6] {
248 let m = sym_to_mat(inertia);
249 let trace = m[0][0] + m[1][1] + m[2][2];
250 let second = [
251 [trace * 0.5 - m[0][0], -m[0][1], -m[0][2]],
252 [-m[1][0], trace * 0.5 - m[1][1], -m[1][2]],
253 [-m[2][0], -m[2][1], trace * 0.5 - m[2][2]],
254 ];
255 let mut scaled = [[0.0f32; 3]; 3];
256 for row in 0..3 {
257 for col in 0..3 {
258 scaled[row][col] = scale[row] * second[row][col] * scale[col];
259 }
260 }
261 let scaled_trace = scaled[0][0] + scaled[1][1] + scaled[2][2];
262 let result = [
263 [scaled_trace - scaled[0][0], -scaled[0][1], -scaled[0][2]],
264 [-scaled[1][0], scaled_trace - scaled[1][1], -scaled[1][2]],
265 [-scaled[2][0], -scaled[2][1], scaled_trace - scaled[2][2]],
266 ];
267 mat_to_sym(result)
268}
269
270fn inertia_rotate(inertia: [f32; 6], q: [f32; 4]) -> [f32; 6] {
271 let r = mat_from_quat(q);
272 let rotated = mat_mul(r, mat_mul(sym_to_mat(inertia), mat_transpose(r)));
273 mat_to_sym(rotated)
274}
275
276pub(crate) fn inertia_inverse(inertia: [f32; 6]) -> [f32; 6] {
277 let [xx, xy, xz, yy, yz, zz] = inertia;
278 let cofactor_xx = yy * zz - yz * yz;
279 let cofactor_xy = xz * yz - xy * zz;
280 let cofactor_xz = xy * yz - xz * yy;
281 let cofactor_yy = xx * zz - xz * xz;
282 let cofactor_yz = xy * xz - xx * yz;
283 let cofactor_zz = xx * yy - xy * xy;
284 let determinant = xx * cofactor_xx + xy * cofactor_xy + xz * cofactor_xz;
285 assert!(
286 determinant > 0.0,
287 "inertia tensor must be positive definite"
288 );
289 [
290 cofactor_xx / determinant,
291 cofactor_xy / determinant,
292 cofactor_xz / determinant,
293 cofactor_yy / determinant,
294 cofactor_yz / determinant,
295 cofactor_zz / determinant,
296 ]
297}
298
299fn mat_from_quat(q: [f32; 4]) -> [[f32; 3]; 3] {
300 let x = q[0];
301 let y = q[1];
302 let z = q[2];
303 let w = q[3];
304 [
305 [
306 1.0 - 2.0 * (y * y + z * z),
307 2.0 * (x * y - w * z),
308 2.0 * (x * z + w * y),
309 ],
310 [
311 2.0 * (x * y + w * z),
312 1.0 - 2.0 * (x * x + z * z),
313 2.0 * (y * z - w * x),
314 ],
315 [
316 2.0 * (x * z - w * y),
317 2.0 * (y * z + w * x),
318 1.0 - 2.0 * (x * x + y * y),
319 ],
320 ]
321}
322
323fn sym_to_mat(sym: [f32; 6]) -> [[f32; 3]; 3] {
324 [
325 [sym[0], sym[1], sym[2]],
326 [sym[1], sym[3], sym[4]],
327 [sym[2], sym[4], sym[5]],
328 ]
329}
330
331fn mat_to_sym(m: [[f32; 3]; 3]) -> [f32; 6] {
332 [
333 m[0][0],
334 (m[0][1] + m[1][0]) * 0.5,
335 (m[0][2] + m[2][0]) * 0.5,
336 m[1][1],
337 (m[1][2] + m[2][1]) * 0.5,
338 m[2][2],
339 ]
340}
341
342fn mat_mul(a: [[f32; 3]; 3], b: [[f32; 3]; 3]) -> [[f32; 3]; 3] {
343 let mut result = [[0.0f32; 3]; 3];
344 for row in 0..3 {
345 for col in 0..3 {
346 result[row][col] =
347 a[row][0] * b[0][col] + a[row][1] * b[1][col] + a[row][2] * b[2][col];
348 }
349 }
350 result
351}
352
353fn mat_transpose(m: [[f32; 3]; 3]) -> [[f32; 3]; 3] {
354 [
355 [m[0][0], m[1][0], m[2][0]],
356 [m[0][1], m[1][1], m[2][1]],
357 [m[0][2], m[1][2], m[2][2]],
358 ]
359}