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 pub fn normal_response(&self, responding_body_index: usize) -> Vec3<Scalar> {
109 let surface_body_index = (responding_body_index + 1) % 2;
110 self.normal[responding_body_index].reflected(self.normal[surface_body_index])
111 }
112}
113
114pub fn intersecting_aabb_for_subdivisions(
115 a: Aabb<Scalar>,
116 b: Aabb<Scalar>,
117) -> Option<Aabb<Scalar>> {
118 Some(a.intersection(b)).filter(|aabb| {
119 aabb.size()
120 .into_iter()
121 .filter(|value| *value >= Scalar::EPSILON)
122 .count()
123 > 1
124 })
125}
126
127pub fn aabb_cell_subdivide(aabb: Aabb<Scalar>) -> [Aabb<Scalar>; 2] {
128 let axis = aabb
129 .size()
130 .into_array()
131 .into_iter()
132 .enumerate()
133 .max_by(|(_, a), (_, b)| a.partial_cmp(b).unwrap_or(Ordering::Equal))
134 .unwrap()
135 .0;
136 let center = (aabb.min[axis] + aabb.max[axis]) * 0.5;
137 let mut a = aabb;
138 let mut b = aabb;
139 a.max[axis] = center;
140 b.min[axis] = center;
141 [a, b]
142}
143
144#[cfg(test)]
145mod tests {
146 #![allow(clippy::approx_constant)]
147
148 use super::*;
149 use crate::{
150 components::{
151 BodyDensityFieldRelation, BodyParentRelation, BodyParticleRelation, PhysicsBody,
152 PhysicsParticle, Position,
153 },
154 density_fields::{DensityFieldBox, aabb::AabbDensityField, sphere::SphereDensityField},
155 };
156 use anput::world::World;
157
158 #[test]
159 fn test_aabb() {
160 let a = Aabb {
161 min: Vec3::new(-2.0, -1.0, 0.0),
162 max: Vec3::new(2.0, 1.0, 0.0),
163 };
164 let b = Aabb {
165 min: Vec3::new(-1.0, -2.0, 0.0),
166 max: Vec3::new(1.0, 2.0, 0.0),
167 };
168
169 let aabb = intersecting_aabb_for_subdivisions(a, b).unwrap();
170 assert_eq!(
171 aabb,
172 Aabb {
173 min: Vec3::new(-1.0, -1.0, 0.0),
174 max: Vec3::new(1.0, 1.0, 0.0),
175 }
176 );
177
178 let subdivided = aabb_cell_subdivide(aabb);
179 assert_eq!(
180 subdivided,
181 [
182 Aabb {
183 min: Vec3 {
184 x: -1.0,
185 y: -1.0,
186 z: 0.0
187 },
188 max: Vec3 {
189 x: 1.0,
190 y: 0.0,
191 z: 0.0
192 }
193 },
194 Aabb {
195 min: Vec3 {
196 x: -1.0,
197 y: 0.0,
198 z: 0.0
199 },
200 max: Vec3 {
201 x: 1.0,
202 y: 1.0,
203 z: 0.0
204 }
205 }
206 ]
207 );
208
209 let a = Aabb {
210 min: Vec3::new(-2.0, 0.0, 0.0),
211 max: Vec3::new(2.0, 0.0, 0.0),
212 };
213 let b = Aabb {
214 min: Vec3::new(-1.0, 0.0, 0.0),
215 max: Vec3::new(1.0, 0.0, 0.0),
216 };
217
218 assert!(intersecting_aabb_for_subdivisions(a, b).is_none());
219
220 let a = Aabb {
221 min: Vec3::new(-2.0, 0.0, -1.0),
222 max: Vec3::new(2.0, 0.0, -1.0),
223 };
224 let b = Aabb {
225 min: Vec3::new(-1.0, 0.0, 1.0),
226 max: Vec3::new(1.0, 0.0, 1.0),
227 };
228
229 assert!(intersecting_aabb_for_subdivisions(a, b).is_none());
230
231 let a = Aabb {
232 min: Vec3::new(-10.0, -10.0, 0.0),
233 max: Vec3::new(0.0, 10.0, 0.0),
234 };
235 let b = Aabb {
236 min: Vec3::new(-5.0, -1.0, 0.0),
237 max: Vec3::new(5.0, 1.0, 0.0),
238 };
239
240 let c = intersecting_aabb_for_subdivisions(a, b).unwrap();
241
242 assert_eq!(
243 c,
244 Aabb {
245 min: Vec3 {
246 x: -5.0,
247 y: -1.0,
248 z: 0.0
249 },
250 max: Vec3 {
251 x: 0.0,
252 y: 1.0,
253 z: 0.0
254 }
255 }
256 );
257
258 let [a, b] = aabb_cell_subdivide(c);
259
260 assert_eq!(
261 a,
262 Aabb {
263 min: Vec3 {
264 x: -5.0,
265 y: -1.0,
266 z: 0.0
267 },
268 max: Vec3 {
269 x: -2.5,
270 y: 1.0,
271 z: 0.0
272 }
273 }
274 );
275 assert_eq!(
276 b,
277 Aabb {
278 min: Vec3 {
279 x: -2.5,
280 y: -1.0,
281 z: 0.0
282 },
283 max: Vec3 {
284 x: 0.0,
285 y: 1.0,
286 z: 0.0
287 }
288 }
289 );
290
291 let [a, b] = aabb_cell_subdivide(a);
292
293 assert_eq!(
294 a,
295 Aabb {
296 min: Vec3 {
297 x: -5.0,
298 y: -1.0,
299 z: 0.0
300 },
301 max: Vec3 {
302 x: -3.75,
303 y: 1.0,
304 z: 0.0
305 }
306 }
307 );
308 assert_eq!(
309 b,
310 Aabb {
311 min: Vec3 {
312 x: -3.75,
313 y: -1.0,
314 z: 0.0
315 },
316 max: Vec3 {
317 x: -2.5,
318 y: 1.0,
319 z: 0.0
320 }
321 }
322 );
323
324 let [a, b] = aabb_cell_subdivide(b);
325
326 assert_eq!(
327 a,
328 Aabb {
329 min: Vec3 {
330 x: -3.75,
331 y: -1.0,
332 z: 0.0
333 },
334 max: Vec3 {
335 x: -2.5,
336 y: 0.0,
337 z: 0.0
338 }
339 }
340 );
341 assert_eq!(
342 b,
343 Aabb {
344 min: Vec3 {
345 x: -3.75,
346 y: 0.0,
347 z: 0.0
348 },
349 max: Vec3 {
350 x: -2.5,
351 y: 1.0,
352 z: 0.0
353 }
354 }
355 );
356 }
357
358 #[test]
359 fn test_shape_overlap_query() {
360 let mut world = World::default();
361
362 let a = world
363 .spawn((
364 PhysicsBody,
365 DensityFieldBox::new(AabbDensityField {
366 aabb: Aabb {
367 min: Vec3::new(-2.0, -2.0, 0.0),
368 max: Vec3::new(0.0, 0.0, 0.0),
369 },
370 density: 1.0,
371 }),
372 ))
373 .unwrap();
374 world
375 .relate::<true, _>(BodyDensityFieldRelation, a, a)
376 .unwrap();
377 world.relate::<true, _>(BodyParentRelation, a, a).unwrap();
378
379 let b = world
380 .spawn((
381 PhysicsBody,
382 PhysicsParticle,
383 Position::new(Vec3::new(0.0, 0.0, 0.0)),
384 DensityFieldBox::new(SphereDensityField::<true>::new_hard(1.0, 1.0)),
385 ))
386 .unwrap();
387 world.relate::<true, _>(BodyParticleRelation, b, b).unwrap();
388 world
389 .relate::<true, _>(BodyDensityFieldRelation, b, b)
390 .unwrap();
391 world.relate::<true, _>(BodyParentRelation, b, b).unwrap();
392
393 let field_a = world
394 .entity::<true, &DensityFieldBox>(a)
395 .unwrap()
396 .as_any()
397 .downcast_ref::<AabbDensityField>()
398 .unwrap();
399 let info_a = BodyAccessInfo::of_world(a, &world);
400
401 let field_b = world
402 .entity::<true, &DensityFieldBox>(b)
403 .unwrap()
404 .as_any()
405 .downcast_ref::<SphereDensityField<true>>()
406 .unwrap();
407 let info_b = BodyAccessInfo::of_world(b, &world);
408
409 let mut cells = vec![];
410 ShapeOverlapQuery {
411 density_threshold: 0.5,
412 voxelization_size_limit: 0.5,
413 ..Default::default()
414 }
415 .query_field_pair([field_a, field_b], [&info_a, &info_b], &mut cells);
416 assert_eq!(
417 cells,
418 vec![
419 ShapeOverlapCell {
420 region: Aabb {
421 min: Vec3 {
422 x: -0.5,
423 y: -0.5,
424 z: 0.0
425 },
426 max: Vec3 {
427 x: 0.0,
428 y: 0.0,
429 z: 0.0
430 }
431 },
432 density: [
433 DensityRange { min: 1.0, max: 1.0 },
434 DensityRange { min: 1.0, max: 1.0 }
435 ],
436 normal: [
437 Vec3 {
438 x: 1.0,
439 y: 0.0,
440 z: 0.0
441 },
442 Vec3 {
443 x: -0.70710677,
444 y: -0.70710677,
445 z: 0.0
446 }
447 ]
448 },
449 ShapeOverlapCell {
450 region: Aabb {
451 min: Vec3 {
452 x: -1.0,
453 y: -0.5,
454 z: 0.0
455 },
456 max: Vec3 {
457 x: -0.5,
458 y: 0.0,
459 z: 0.0
460 }
461 },
462 density: [
463 DensityRange { min: 1.0, max: 1.0 },
464 DensityRange { min: 0.0, max: 1.0 }
465 ],
466 normal: [
467 Vec3 {
468 x: 0.0,
469 y: 1.0,
470 z: 0.0
471 },
472 Vec3 {
473 x: -0.94868326,
474 y: -0.31622776,
475 z: 0.0
476 }
477 ]
478 },
479 ShapeOverlapCell {
480 region: Aabb {
481 min: Vec3 {
482 x: -0.5,
483 y: -1.0,
484 z: 0.0
485 },
486 max: Vec3 {
487 x: 0.0,
488 y: -0.5,
489 z: 0.0
490 }
491 },
492 density: [
493 DensityRange { min: 1.0, max: 1.0 },
494 DensityRange { min: 0.0, max: 1.0 }
495 ],
496 normal: [
497 Vec3 {
498 x: 1.0,
499 y: 0.0,
500 z: 0.0
501 },
502 Vec3 {
503 x: -0.31622776,
504 y: -0.94868326,
505 z: 0.0
506 }
507 ]
508 },
509 ShapeOverlapCell {
510 region: Aabb {
511 min: Vec3 {
512 x: -1.0,
513 y: -1.0,
514 z: 0.0
515 },
516 max: Vec3 {
517 x: -0.5,
518 y: -0.5,
519 z: 0.0
520 }
521 },
522 density: [
523 DensityRange { min: 1.0, max: 1.0 },
524 DensityRange { min: 0.0, max: 1.0 }
525 ],
526 normal: [
527 Vec3 {
528 x: 1.0,
529 y: 0.0,
530 z: 0.0
531 },
532 Vec3 {
533 x: -0.7071068,
534 y: -0.7071068,
535 z: 0.0
536 }
537 ]
538 }
539 ],
540 );
541 }
542}