1use std::cmp::Ordering;
2
3use crate::{
4 Scalar,
5 components::BodyAccessInfo,
6 density_fields::{DensityField, DensityRange},
7};
8use vek::{Aabb, Vec3};
9
10#[derive(Debug, Clone, PartialEq)]
11pub struct ShapeOverlapQuery {
12 pub density_threshold: Scalar,
13 pub voxelization_size_limit: Scalar,
14 pub region_limit: Option<Aabb<Scalar>>,
15 pub depth_limit: usize,
16}
17
18impl Default for ShapeOverlapQuery {
19 fn default() -> Self {
20 Self {
21 density_threshold: 0.5,
22 voxelization_size_limit: 1.0,
23 region_limit: None,
24 depth_limit: usize::MAX,
25 }
26 }
27}
28
29impl ShapeOverlapQuery {
30 pub fn query_field_pair(
31 &self,
32 field: [&dyn DensityField; 2],
33 info: [&BodyAccessInfo; 2],
34 result: &mut Vec<ShapeOverlapCell>,
35 ) -> Option<Aabb<Scalar>> {
36 self.query_field_pair_mapped(field, info, result, |cell| cell)
37 }
38
39 pub fn query_field_pair_mapped<T>(
40 &self,
41 field: [&dyn DensityField; 2],
42 info: [&BodyAccessInfo; 2],
43 result: &mut Vec<T>,
44 converter: impl Fn(ShapeOverlapCell) -> T,
45 ) -> Option<Aabb<Scalar>> {
46 let mut a = field[0].aabb(info[0]);
47 let mut b = field[1].aabb(info[1]);
48 if let Some(region_limit) = self.region_limit {
49 a = a.intersection(region_limit);
50 b = b.intersection(region_limit);
51 }
52 let aabb = intersecting_aabb_for_subdivisions(a, b)?;
53 let mut stack = vec![(aabb, 0)];
54 while let Some((region, depth)) = stack.pop() {
55 let a = field[0].density_at_region(region, info[0]);
56 let b = field[1].density_at_region(region, info[1]);
57 if a.max.min(b.max) <= self.density_threshold {
58 continue;
59 }
60 if region
61 .size()
62 .into_iter()
63 .any(|v| v > self.voxelization_size_limit)
64 && (a.has_separation() || b.has_separation())
65 && depth < self.depth_limit
66 {
67 stack.extend(aabb_cell_subdivide(region).map(|region| (region, depth + 1)));
68 continue;
69 }
70 let center = region.center();
71 let density = [a, b];
72 let resolution = Vec3::from(region.size()) * 0.5;
73 let normal =
74 [0, 1].map(|index| field[index].normal_at_point(center, resolution, info[index]));
75 let normal = match normal.map(|normal| normal.is_approx_zero()) {
78 [true, true] | [false, false] => normal,
79 [true, false] => [-normal[1], normal[1]],
80 [false, true] => [normal[0], -normal[0]],
81 };
82 result.push(converter(ShapeOverlapCell {
83 region,
84 density,
85 normal,
86 }));
87 }
88 Some(aabb)
89 }
90}
91
92#[derive(Debug, Clone, PartialEq)]
93pub struct ShapeOverlapCell {
94 pub region: Aabb<Scalar>,
95 pub density: [DensityRange; 2],
96 pub normal: [Vec3<Scalar>; 2],
97}
98
99impl ShapeOverlapCell {
100 pub fn area(&self) -> Scalar {
101 self.region
102 .size()
103 .into_iter()
104 .filter(|v| *v > Scalar::EPSILON)
105 .product::<Scalar>()
106 }
107}
108
109pub fn intersecting_aabb_for_subdivisions(
110 a: Aabb<Scalar>,
111 b: Aabb<Scalar>,
112) -> Option<Aabb<Scalar>> {
113 Some(a.intersection(b)).filter(|aabb| {
114 aabb.size()
115 .into_iter()
116 .filter(|value| *value >= Scalar::EPSILON)
117 .count()
118 > 1
119 })
120}
121
122pub fn aabb_cell_subdivide(aabb: Aabb<Scalar>) -> [Aabb<Scalar>; 2] {
123 let axis = aabb
124 .size()
125 .into_array()
126 .into_iter()
127 .enumerate()
128 .max_by(|(_, a), (_, b)| a.partial_cmp(b).unwrap_or(Ordering::Equal))
129 .unwrap()
130 .0;
131 let center = (aabb.min[axis] + aabb.max[axis]) * 0.5;
132 let mut a = aabb;
133 let mut b = aabb;
134 a.max[axis] = center;
135 b.min[axis] = center;
136 [a, b]
137}
138
139#[cfg(test)]
140mod tests {
141 #![allow(clippy::approx_constant)]
142
143 use super::*;
144 use crate::{
145 components::{
146 BodyDensityFieldRelation, BodyParentRelation, BodyParticleRelation, PhysicsBody,
147 PhysicsParticle, Position,
148 },
149 density_fields::{DensityFieldBox, aabb::AabbDensityField, sphere::SphereDensityField},
150 };
151 use anput::world::World;
152
153 #[test]
154 fn test_aabb() {
155 let a = Aabb {
156 min: Vec3::new(-2.0, -1.0, 0.0),
157 max: Vec3::new(2.0, 1.0, 0.0),
158 };
159 let b = Aabb {
160 min: Vec3::new(-1.0, -2.0, 0.0),
161 max: Vec3::new(1.0, 2.0, 0.0),
162 };
163
164 let aabb = intersecting_aabb_for_subdivisions(a, b).unwrap();
165 assert_eq!(
166 aabb,
167 Aabb {
168 min: Vec3::new(-1.0, -1.0, 0.0),
169 max: Vec3::new(1.0, 1.0, 0.0),
170 }
171 );
172
173 let subdivided = aabb_cell_subdivide(aabb);
174 assert_eq!(
175 subdivided,
176 [
177 Aabb {
178 min: Vec3 {
179 x: -1.0,
180 y: -1.0,
181 z: 0.0
182 },
183 max: Vec3 {
184 x: 1.0,
185 y: 0.0,
186 z: 0.0
187 }
188 },
189 Aabb {
190 min: Vec3 {
191 x: -1.0,
192 y: 0.0,
193 z: 0.0
194 },
195 max: Vec3 {
196 x: 1.0,
197 y: 1.0,
198 z: 0.0
199 }
200 }
201 ]
202 );
203
204 let a = Aabb {
205 min: Vec3::new(-2.0, 0.0, 0.0),
206 max: Vec3::new(2.0, 0.0, 0.0),
207 };
208 let b = Aabb {
209 min: Vec3::new(-1.0, 0.0, 0.0),
210 max: Vec3::new(1.0, 0.0, 0.0),
211 };
212
213 assert!(intersecting_aabb_for_subdivisions(a, b).is_none());
214
215 let a = Aabb {
216 min: Vec3::new(-2.0, 0.0, -1.0),
217 max: Vec3::new(2.0, 0.0, -1.0),
218 };
219 let b = Aabb {
220 min: Vec3::new(-1.0, 0.0, 1.0),
221 max: Vec3::new(1.0, 0.0, 1.0),
222 };
223
224 assert!(intersecting_aabb_for_subdivisions(a, b).is_none());
225
226 let a = Aabb {
227 min: Vec3::new(-10.0, -10.0, 0.0),
228 max: Vec3::new(0.0, 10.0, 0.0),
229 };
230 let b = Aabb {
231 min: Vec3::new(-5.0, -1.0, 0.0),
232 max: Vec3::new(5.0, 1.0, 0.0),
233 };
234
235 let c = intersecting_aabb_for_subdivisions(a, b).unwrap();
236
237 assert_eq!(
238 c,
239 Aabb {
240 min: Vec3 {
241 x: -5.0,
242 y: -1.0,
243 z: 0.0
244 },
245 max: Vec3 {
246 x: 0.0,
247 y: 1.0,
248 z: 0.0
249 }
250 }
251 );
252
253 let [a, b] = aabb_cell_subdivide(c);
254
255 assert_eq!(
256 a,
257 Aabb {
258 min: Vec3 {
259 x: -5.0,
260 y: -1.0,
261 z: 0.0
262 },
263 max: Vec3 {
264 x: -2.5,
265 y: 1.0,
266 z: 0.0
267 }
268 }
269 );
270 assert_eq!(
271 b,
272 Aabb {
273 min: Vec3 {
274 x: -2.5,
275 y: -1.0,
276 z: 0.0
277 },
278 max: Vec3 {
279 x: 0.0,
280 y: 1.0,
281 z: 0.0
282 }
283 }
284 );
285
286 let [a, b] = aabb_cell_subdivide(a);
287
288 assert_eq!(
289 a,
290 Aabb {
291 min: Vec3 {
292 x: -5.0,
293 y: -1.0,
294 z: 0.0
295 },
296 max: Vec3 {
297 x: -3.75,
298 y: 1.0,
299 z: 0.0
300 }
301 }
302 );
303 assert_eq!(
304 b,
305 Aabb {
306 min: Vec3 {
307 x: -3.75,
308 y: -1.0,
309 z: 0.0
310 },
311 max: Vec3 {
312 x: -2.5,
313 y: 1.0,
314 z: 0.0
315 }
316 }
317 );
318
319 let [a, b] = aabb_cell_subdivide(b);
320
321 assert_eq!(
322 a,
323 Aabb {
324 min: Vec3 {
325 x: -3.75,
326 y: -1.0,
327 z: 0.0
328 },
329 max: Vec3 {
330 x: -2.5,
331 y: 0.0,
332 z: 0.0
333 }
334 }
335 );
336 assert_eq!(
337 b,
338 Aabb {
339 min: Vec3 {
340 x: -3.75,
341 y: 0.0,
342 z: 0.0
343 },
344 max: Vec3 {
345 x: -2.5,
346 y: 1.0,
347 z: 0.0
348 }
349 }
350 );
351 }
352
353 #[test]
354 fn test_shape_overlap_query() {
355 let mut world = World::default();
356
357 let a = world
358 .spawn((
359 PhysicsBody,
360 DensityFieldBox::new(AabbDensityField {
361 aabb: Aabb {
362 min: Vec3::new(-2.0, -2.0, 0.0),
363 max: Vec3::new(0.0, 0.0, 0.0),
364 },
365 density: 1.0,
366 }),
367 ))
368 .unwrap();
369 world
370 .relate::<true, _>(BodyDensityFieldRelation, a, a)
371 .unwrap();
372 world.relate::<true, _>(BodyParentRelation, a, a).unwrap();
373
374 let b = world
375 .spawn((
376 PhysicsBody,
377 PhysicsParticle,
378 Position::new(Vec3::new(0.0, 0.0, 0.0)),
379 DensityFieldBox::new(SphereDensityField::<true>::new_hard(1.0, 1.0)),
380 ))
381 .unwrap();
382 world.relate::<true, _>(BodyParticleRelation, b, b).unwrap();
383 world
384 .relate::<true, _>(BodyDensityFieldRelation, b, b)
385 .unwrap();
386 world.relate::<true, _>(BodyParentRelation, b, b).unwrap();
387
388 let field_a = world
389 .entity::<true, &DensityFieldBox>(a)
390 .unwrap()
391 .as_any()
392 .downcast_ref::<AabbDensityField>()
393 .unwrap();
394 let info_a = BodyAccessInfo::of_world(a, &world);
395
396 let field_b = world
397 .entity::<true, &DensityFieldBox>(b)
398 .unwrap()
399 .as_any()
400 .downcast_ref::<SphereDensityField<true>>()
401 .unwrap();
402 let info_b = BodyAccessInfo::of_world(b, &world);
403
404 let mut cells = vec![];
405 ShapeOverlapQuery {
406 density_threshold: 0.5,
407 voxelization_size_limit: 0.5,
408 ..Default::default()
409 }
410 .query_field_pair([field_a, field_b], [&info_a, &info_b], &mut cells);
411 assert_eq!(
412 cells,
413 vec![
414 ShapeOverlapCell {
415 region: Aabb {
416 min: Vec3 {
417 x: -0.5,
418 y: -0.5,
419 z: 0.0
420 },
421 max: Vec3 {
422 x: 0.0,
423 y: 0.0,
424 z: 0.0
425 }
426 },
427 density: [
428 DensityRange { min: 1.0, max: 1.0 },
429 DensityRange { min: 1.0, max: 1.0 }
430 ],
431 normal: [
432 Vec3 {
433 x: 1.0,
434 y: 0.0,
435 z: 0.0
436 },
437 Vec3 {
438 x: -0.70710677,
439 y: -0.70710677,
440 z: 0.0
441 }
442 ]
443 },
444 ShapeOverlapCell {
445 region: Aabb {
446 min: Vec3 {
447 x: -1.0,
448 y: -0.5,
449 z: 0.0
450 },
451 max: Vec3 {
452 x: -0.5,
453 y: 0.0,
454 z: 0.0
455 }
456 },
457 density: [
458 DensityRange { min: 1.0, max: 1.0 },
459 DensityRange { min: 0.0, max: 1.0 }
460 ],
461 normal: [
462 Vec3 {
463 x: 0.0,
464 y: 1.0,
465 z: 0.0
466 },
467 Vec3 {
468 x: -0.94868326,
469 y: -0.31622776,
470 z: 0.0
471 }
472 ]
473 },
474 ShapeOverlapCell {
475 region: Aabb {
476 min: Vec3 {
477 x: -0.5,
478 y: -1.0,
479 z: 0.0
480 },
481 max: Vec3 {
482 x: 0.0,
483 y: -0.5,
484 z: 0.0
485 }
486 },
487 density: [
488 DensityRange { min: 1.0, max: 1.0 },
489 DensityRange { min: 0.0, max: 1.0 }
490 ],
491 normal: [
492 Vec3 {
493 x: 1.0,
494 y: 0.0,
495 z: 0.0
496 },
497 Vec3 {
498 x: -0.31622776,
499 y: -0.94868326,
500 z: 0.0
501 }
502 ]
503 },
504 ShapeOverlapCell {
505 region: Aabb {
506 min: Vec3 {
507 x: -1.0,
508 y: -1.0,
509 z: 0.0
510 },
511 max: Vec3 {
512 x: -0.5,
513 y: -0.5,
514 z: 0.0
515 }
516 },
517 density: [
518 DensityRange { min: 1.0, max: 1.0 },
519 DensityRange { min: 0.0, max: 1.0 }
520 ],
521 normal: [
522 Vec3 {
523 x: 1.0,
524 y: 0.0,
525 z: 0.0
526 },
527 Vec3 {
528 x: -0.7071068,
529 y: -0.7071068,
530 z: 0.0
531 }
532 ]
533 }
534 ],
535 );
536 }
537}