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