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