1use fast_mesh_bvh::Bvh;
4use fast_mesh_query::{ClosestOptions, QueryError, Rigid, closest_point};
5use fast_mesh_store::{Mesh, par};
6
7use crate::distance::{PAR_MIN_POINTS, POINT_CHUNK, hit_value, point_value};
8use crate::{FieldsError, Result, SignData, SignMode};
9
10#[derive(Debug, Clone, Copy)]
12pub struct Target<'a> {
13 pub mesh: &'a Mesh,
15 pub bvh: &'a Bvh,
17 pub sign: &'a SignData,
19 pub to_local: Rigid,
21}
22
23fn pruned_value(t: &Target<'_>, q: [f32; 3], best: f32, bound: f32) -> Result<Option<f32>> {
44 let search =
45 |max_distance: f32| closest_point(t.mesh, t.bvh, q, &ClosestOptions { max_distance });
46 if best < 0.0 {
47 let b = -best;
48 if b < bound {
49 match search(b)? {
50 Some(hit) if hit.distance <= b => {
51 #[cfg(test)]
52 pruned::add();
53 return Ok(None);
54 }
55 Some(hit) => return Ok(Some(hit_value(t.mesh, t.bvh, t.sign, q, Some(&hit)))),
56 None => {}
57 }
58 }
59 } else if t.sign.mode() == SignMode::Winding {
60 let b = best.next_up();
61 if b < bound {
62 match search(b)? {
63 Some(hit) => return Ok(Some(hit_value(t.mesh, t.bvh, t.sign, q, Some(&hit)))),
64 None if t.sign.winding_number(t.mesh, t.bvh, q) < 0.5 => {
65 #[cfg(test)]
66 pruned::add();
67 return Ok(None);
68 }
69 None => {}
70 }
71 }
72 }
73 Ok(Some(point_value(t.mesh, t.bvh, t.sign, q, bound)?))
74}
75
76#[cfg(test)]
78pub(crate) mod pruned {
79 use std::cell::Cell;
80
81 thread_local! {
82 static PRUNED: Cell<usize> = const { Cell::new(0) };
83 }
84
85 pub(crate) fn add() {
86 PRUNED.with(|c| c.set(c.get() + 1));
87 }
88
89 pub(crate) fn take() -> usize {
90 PRUNED.with(|c| c.replace(0))
91 }
92}
93
94fn round_up(x: f64) -> f32 {
96 let r = x as f32;
97 if f64::from(r) < x { r.next_up() } else { r }
98}
99
100pub fn signed_distances_multi(
134 targets: &[Target<'_>],
135 points: &[[f32; 3]],
136 max_distance: f64,
137 out: &mut [f32],
138 nearest: &mut [u32],
139) -> Result<()> {
140 if points.len() != out.len() || points.len() != nearest.len() {
141 return Err(FieldsError::InvalidInput(
142 "points, out and nearest must have the same length",
143 ));
144 }
145 if max_distance.is_nan() || max_distance < 0.0 {
146 return Err(FieldsError::InvalidInput(
147 "max_distance must be zero or greater, and not NaN",
148 ));
149 }
150 if targets.len() >= u32::MAX as usize {
151 return Err(FieldsError::InvalidInput("too many targets"));
152 }
153 if !points.iter().flatten().all(|c| c.is_finite()) {
154 return Err(FieldsError::InvalidInput("points must be finite"));
155 }
156 for t in targets {
157 if t.bvh.mesh_uid() != t.mesh.uid() {
158 return Err(QueryError::MeshMismatch.into());
159 }
160 if !t.bvh.is_current(t.mesh) {
161 return Err(QueryError::StaleBvh.into());
162 }
163 if !t.sign.is_current(t.mesh, t.bvh) {
164 return Err(FieldsError::InvalidInput(
165 "sign data is not current for this mesh and BVH",
166 ));
167 }
168 let moved = |&p: &[f32; 3]| t.to_local.apply(p).map(|c| c as f32);
169 if !points.iter().flat_map(moved).all(f32::is_finite) {
170 return Err(FieldsError::InvalidInput("points must be finite"));
171 }
172 }
173 let bound = round_up(max_distance);
174 let value = |p: [f32; 3]| -> Result<(f32, u32)> {
175 let (mut best, mut at) = (f32::INFINITY, u32::MAX);
176 for (k, t) in targets.iter().enumerate() {
177 let q = t.to_local.apply(p).map(|c| c as f32);
178 let found = if best.is_finite() {
179 pruned_value(t, q, best, bound)?
180 } else {
181 Some(point_value(t.mesh, t.bvh, t.sign, q, bound)?)
182 };
183 let Some(mut v) = found else {
184 continue;
185 };
186 if f64::from(v.abs()) > max_distance {
187 v = f32::INFINITY;
188 }
189 if v < best {
191 (best, at) = (v, k as u32);
192 }
193 }
194 Ok((best, at))
195 };
196 let work = points.len().saturating_mul(targets.len());
197 if work < PAR_MIN_POINTS || !par::enabled() || par::force_sequential() {
198 for ((&p, v), n) in points.iter().zip(out.iter_mut()).zip(nearest.iter_mut()) {
199 (*v, *n) = value(p)?;
200 }
201 return Ok(());
202 }
203 let oom = || FieldsError::OutOfMemory;
205 let parts = par::map_range(points.len(), POINT_CHUNK, 0, oom, |r| {
206 let mut part = Vec::new();
207 part.try_reserve_exact(r.len()).map_err(|_| oom())?;
208 for &p in &points[r] {
209 part.push(value(p)?);
210 }
211 Ok(part)
212 })?;
213 let chunks = out
214 .chunks_mut(POINT_CHUNK)
215 .zip(nearest.chunks_mut(POINT_CHUNK));
216 for ((vs, ns), part) in chunks.zip(parts) {
217 for ((v, n), (x, k)) in vs.iter_mut().zip(ns.iter_mut()).zip(part) {
218 (*v, *n) = (x, k);
219 }
220 }
221 Ok(())
222}
223
224#[cfg(test)]
225mod tests {
226 use fast_mesh_query::{QueryError, Rigid};
227 use fast_mesh_store::par;
228
229 use super::*;
230 use crate::test_util::*;
231 use crate::{SignMode, signed_distances};
232
233 fn turned(axis: usize, deg: f64, t: [f64; 3]) -> Rigid {
236 let (s, c) = deg.to_radians().sin_cos();
237 let (i, j) = ((axis + 1) % 3, (axis + 2) % 3);
238 let mut rotation = [[0.0; 3]; 3];
239 rotation[axis][axis] = 1.0;
240 rotation[i][i] = c;
241 rotation[i][j] = -s;
242 rotation[j][i] = s;
243 rotation[j][j] = c;
244 Rigid {
245 rotation,
246 translation: t,
247 }
248 }
249
250 fn reference(
253 targets: &[Target<'_>],
254 points: &[[f32; 3]],
255 max_distance: f64,
256 ) -> (Vec<f32>, Vec<u32>) {
257 let mut best = vec![f32::INFINITY; points.len()];
258 let mut at = vec![u32::MAX; points.len()];
259 for (k, t) in targets.iter().enumerate() {
260 let moved: Vec<[f32; 3]> = points
261 .iter()
262 .map(|&p| t.to_local.apply(p).map(|c| c as f32))
263 .collect();
264 let mut out = vec![f32::NAN; points.len()];
265 let bound = round_up(max_distance);
266 signed_distances(t.mesh, t.bvh, t.sign, &moved, bound, &mut out).unwrap();
267 for i in 0..points.len() {
268 let v = if f64::from(out[i].abs()) > max_distance {
269 f32::INFINITY
270 } else {
271 out[i]
272 };
273 if v < best[i] {
274 best[i] = v;
275 at[i] = k as u32;
276 }
277 }
278 }
279 (best, at)
280 }
281
282 fn bits(values: &[f32]) -> Vec<u32> {
283 values.iter().map(|x| x.to_bits()).collect()
284 }
285
286 struct Setup {
289 meshes: Vec<Mesh>,
290 bvhs: Vec<Bvh>,
291 places: Vec<Rigid>,
292 points: Vec<[f32; 3]>,
293 }
294
295 fn setup() -> Setup {
296 let mut meshes = vec![cube(), icosphere(), torus(), terrain(12)];
297 let places = vec![
298 turned(2, 20.0, [0.3, 0.0, 0.0]),
299 turned(0, 35.0, [-0.6, 0.4, 0.2]),
300 turned(1, -50.0, [0.0, -0.5, 0.4]),
301 turned(2, 0.0, [-6.0, -6.0, -2.0]),
303 ];
304 let bvhs = meshes.iter_mut().map(|m| build_bvh(m)).collect();
305 let mut rng = Lcg(91);
306 let points = (0..3000)
307 .map(|_| rng.point([-2.5, -2.5, -2.5], [2.5, 2.5, 2.5]))
308 .collect();
309 Setup {
310 meshes,
311 bvhs,
312 places,
313 points,
314 }
315 }
316
317 fn signs(s: &mut Setup, mode: SignMode) -> Vec<SignData> {
318 s.meshes
319 .iter_mut()
320 .zip(&s.bvhs)
321 .map(|(m, b)| SignData::build(m, b, mode).unwrap())
322 .collect()
323 }
324
325 fn targets<'a>(s: &'a Setup, signs: &'a [SignData], order: &[usize]) -> Vec<Target<'a>> {
326 order
327 .iter()
328 .map(|&k| Target {
329 mesh: &s.meshes[k],
330 bvh: &s.bvhs[k],
331 sign: &signs[k],
332 to_local: s.places[k].inverse(),
334 })
335 .collect()
336 }
337
338 fn run(targets: &[Target<'_>], points: &[[f32; 3]], md: f64) -> (Vec<f32>, Vec<u32>) {
339 let mut out = vec![f32::NAN; points.len()];
340 let mut nearest = vec![7; points.len()];
341 signed_distances_multi(targets, points, md, &mut out, &mut nearest).unwrap();
342 (out, nearest)
343 }
344
345 #[test]
346 fn matches_min_over_single_targets() {
347 let mut s = setup();
348 for mode in [SignMode::Pseudonormal, SignMode::Winding] {
349 let signs = signs(&mut s, mode);
350 for order in [&[0, 1, 2, 3][..], &[3, 1, 0], &[2], &[]] {
351 let targets = targets(&s, &signs, order);
352 for md in [f64::INFINITY, 0.3, 0.1 + 1e-9, 0.0] {
353 let (want, want_at) = reference(&targets, &s.points, md);
354 let (got, at) = run(&targets, &s.points, md);
355 assert_eq!(bits(&got), bits(&want), "{mode:?} {order:?} {md}");
356 assert_eq!(at, want_at, "{mode:?} {order:?} {md}");
357 }
358 }
359 let (got, at) = run(&targets(&s, &signs, &[0, 1, 2, 3]), &s.points, 0.3);
361 assert!(got.iter().any(|&v| v < 0.0));
362 assert!(got.iter().any(|&v| v > 0.0 && v.is_finite()));
363 assert!(got.contains(&f32::INFINITY));
364 for k in 0..4 {
365 assert!(at.contains(&k), "{mode:?}: target {k} never nearest");
366 }
367 assert!(
368 got.iter()
369 .zip(&at)
370 .all(|(v, &k)| v.is_finite() == (k != u32::MAX))
371 );
372 }
373 }
374
375 #[test]
376 fn pruning_skips_targets_in_both_modes() {
377 par::set_force_sequential(true);
379 let mut s = setup();
380 for mode in [SignMode::Pseudonormal, SignMode::Winding] {
381 let signs = signs(&mut s, mode);
382 let targets = targets(&s, &signs, &[0, 1, 2, 3]);
383 for md in [f64::INFINITY, 0.3] {
384 pruned::take();
385 let (got, at) = run(&targets, &s.points, md);
386 let skipped = pruned::take();
387 let (want, want_at) = reference(&targets, &s.points, md);
388 assert_eq!(bits(&got), bits(&want), "{mode:?} {md}");
389 assert_eq!(at, want_at, "{mode:?} {md}");
390 assert!(skipped > 0, "{mode:?} {md}: nothing pruned");
391 }
392 }
393 par::set_force_sequential(false);
394 }
395
396 fn check_orders(
400 targets: &[Target<'_>],
401 points: &[[f32; 3]],
402 orders: &[&[usize]],
403 mds: &[f64],
404 what: &str,
405 ) -> usize {
406 let mut skipped = 0;
407 for order in orders {
408 let picked: Vec<Target<'_>> = order.iter().map(|&k| targets[k]).collect();
409 for &md in mds {
410 pruned::take();
411 let (got, at) = run(&picked, points, md);
412 skipped += pruned::take();
413 let (want, want_at) = reference(&picked, points, md);
414 assert_eq!(bits(&got), bits(&want), "{what} {order:?} {md}");
415 assert_eq!(at, want_at, "{what} {order:?} {md}");
416 }
417 }
418 skipped
419 }
420
421 fn terrain_points(measured: &Mesh) -> Vec<[f32; 3]> {
423 let mut rng = Lcg(17);
424 let mut points = measured.positions().to_vec();
425 for i in 0..2000 {
426 let s = surface_point(measured, &mut rng);
427 let near = if i % 2 == 0 { 0.0 } else { 0.4 };
428 let d = rng.point([-near; 3], [near; 3]);
429 points.push(std::array::from_fn(|k| s[k] + d[k]));
430 }
431 points
432 }
433
434 #[test]
435 fn matches_on_penetrating_terrain() {
436 par::set_force_sequential(true);
438 let measured = terrain(24);
442 let measured = mesh(
443 &measured
444 .positions()
445 .iter()
446 .map(|p| [p[0] * 0.5, p[1] * 0.5, p[2]])
447 .collect::<Vec<_>>(),
448 measured.triangles(),
449 );
450 let points = terrain_points(&measured);
451 let mut meshes = [terrain(12), terrain(12), cube(), icosphere(), torus()];
452 let places = [
453 turned(0, 8.0, [0.0, 0.0, -0.6]),
455 turned(2, 90.0, [12.0, 0.0, 0.2]),
457 turned(2, 30.0, [4.0, 4.0, 0.1]),
459 turned(0, 20.0, [8.0, 6.0, -0.2]),
460 turned(0, 90.0, [6.0, 9.5, 0.0]),
462 ];
463 let bvhs: Vec<Bvh> = meshes.iter_mut().map(|m| build_bvh(m)).collect();
464 let orders: [&[usize]; 4] = [
465 &[0, 1, 2, 3, 4],
466 &[2, 0, 3, 1, 4],
467 &[4, 3, 2, 1, 0],
468 &[1, 0],
469 ];
470 for mode in [SignMode::Pseudonormal, SignMode::Winding, SignMode::Auto] {
471 let signs: Vec<SignData> = meshes
472 .iter_mut()
473 .zip(&bvhs)
474 .map(|(m, b)| SignData::build(m, b, mode).unwrap())
475 .collect();
476 let targets: Vec<Target<'_>> = (0..meshes.len())
477 .map(|k| Target {
478 mesh: &meshes[k],
479 bvh: &bvhs[k],
480 sign: &signs[k],
481 to_local: places[k].inverse(),
482 })
483 .collect();
484 let mds = [f64::INFINITY, 0.4];
485 let skipped = check_orders(&targets, &points, &orders, &mds, &format!("{mode:?}"));
486 assert!(skipped > 0, "{mode:?}: nothing pruned");
487 for t in &targets {
489 let (v, _) = run(&[*t], &points, f64::INFINITY);
490 assert!(v.iter().any(|&x| x < 0.0), "{mode:?}: no negative");
491 assert!(v.iter().any(|&x| x > 0.0), "{mode:?}: no positive");
492 }
493 }
494 par::set_force_sequential(false);
495 }
496
497 fn flip_some(m: &Mesh, every: usize) -> Mesh {
499 let mut t = m.triangles().to_vec();
500 for tri in t.iter_mut().step_by(every) {
501 tri.swap(1, 2);
502 }
503 mesh(m.positions(), &t)
504 }
505
506 #[test]
507 fn matches_with_winding_targets() {
508 par::set_force_sequential(true);
510 let mut s = setup();
511 s.meshes.push(flip_some(&cube(), 12));
514 s.meshes.push(flip_some(&icosphere(), 7));
515 s.meshes.push(flip_some(&torus(), 5));
516 s.places.push(turned(1, 15.0, [-0.2, 0.6, -0.5]));
517 s.places.push(turned(2, -25.0, [0.8, -0.3, 0.6]));
518 s.places.push(turned(0, 60.0, [0.4, 0.5, -0.3]));
519 s.bvhs = s.meshes.iter_mut().map(|m| build_bvh(m)).collect();
520 let signs = signs(&mut s, SignMode::Auto);
521 let modes: Vec<SignMode> = signs.iter().map(SignData::mode).collect();
522 use SignMode::{Pseudonormal as P, Winding as W};
523 assert_eq!(modes, [P, P, P, P, W, W, W]);
524 let all = targets(&s, &signs, &[0, 1, 2, 3, 4, 5, 6]);
525 let orders: [&[usize]; 5] = [
526 &[0, 1, 2, 3, 4, 5, 6],
527 &[4, 5, 6, 0, 1, 2, 3],
528 &[6, 3, 5, 0, 4],
529 &[4, 5, 6],
530 &[1, 5],
531 ];
532 let mds = [f64::INFINITY, 0.3];
533 let skipped = check_orders(&all, &s.points, &orders, &mds, "auto");
534 assert!(skipped > 0, "nothing pruned");
535 for t in &all[4..] {
537 let (v, _) = run(&[*t], &s.points, f64::INFINITY);
538 assert!(v.iter().any(|&x| x < 0.0) && v.iter().any(|&x| x > 0.0));
539 }
540
541 for (first, then) in [(1, 5), (0, 4), (2, 6)] {
545 let pair = [all[first], all[then]];
546 let (v, _) = run(&pair, &s.points, f64::INFINITY);
547 let outside: Vec<[f32; 3]> = s
548 .points
549 .iter()
550 .zip(&v)
551 .filter(|(_, x)| **x > 0.0)
552 .map(|(&p, _)| p)
553 .collect();
554 assert!(!outside.is_empty());
555 let skipped = check_orders(&pair, &outside, &[&[0, 1]], &mds, "outside");
556 assert!(skipped > 0, "{first} then {then}: nothing pruned");
557 }
558 par::set_force_sequential(false);
559 }
560
561 #[test]
562 fn ties_go_to_the_lowest_index() {
563 let mut s = setup();
564 let signs = signs(&mut s, SignMode::Auto);
565 let targets = targets(&s, &signs, &[1, 1, 0]);
567 let (got, at) = run(&targets, &s.points, f64::INFINITY);
568 let (want, want_at) = reference(&targets, &s.points, f64::INFINITY);
569 assert_eq!(bits(&got), bits(&want));
570 assert_eq!(at, want_at);
571 assert!(!at.contains(&1));
572 assert!(at.contains(&0) && at.contains(&2));
573 }
574
575 const POOLS: [usize; 3] = [1, 2, 8];
577
578 #[test]
579 fn bit_identical_in_parallel() {
580 let mut s = setup();
581 let mut rng = Lcg(5);
582 s.points = (0..20_000)
583 .map(|_| rng.point([-2.5, -2.5, -2.5], [2.5, 2.5, 2.5]))
584 .collect();
585 for mode in [SignMode::Pseudonormal, SignMode::Winding] {
586 let signs = signs(&mut s, mode);
587 let targets = targets(&s, &signs, &[0, 1, 2, 3]);
588 for md in [f64::INFINITY, 0.3] {
589 let go = || {
590 let (v, n) = run(&targets, &s.points, md);
591 (bits(&v), n)
592 };
593 par::set_force_sequential(true);
594 let reference = go();
595 par::set_force_sequential(false);
596 if !par::enabled() {
597 continue;
598 }
599 for k in POOLS {
600 assert!(par::with_threads(k, go) == reference, "{mode:?} {md}: {k}");
601 }
602 let before = par::parallel_entries();
603 assert!(go() == reference, "{mode:?} {md}: global pool");
604 assert!(par::parallel_entries() > before, "ran sequentially");
605 }
606 }
607 }
608
609 #[test]
610 fn invalid_input_writes_nothing() {
611 let mut s = setup();
612 let signs = signs(&mut s, SignMode::Auto);
613 let targets = targets(&s, &signs, &[0, 1]);
614 let points = &s.points[..4];
615 let mut out = [7.0f32; 4];
616 let mut nearest = [7u32; 4];
617 let invalid = |r: Result<()>| matches!(r, Err(FieldsError::InvalidInput(_)));
618 let mut call = |t: &[Target<'_>], p: &[[f32; 3]], md: f64, n: usize| {
619 signed_distances_multi(t, p, md, &mut out[..n], &mut nearest[..n])
620 };
621 assert!(invalid(call(&targets, points, f64::NAN, 4)));
622 assert!(invalid(call(&targets, points, -1.0, 4)));
623 assert!(invalid(call(&targets, &points[..3], 1.0, 4)));
624 let mut bad = points.to_vec();
625 bad[2][0] = f32::NAN;
626 assert!(invalid(call(&targets, &bad, 1.0, 4)));
627 let mut far = targets.clone();
629 far[1].to_local.translation = [f64::from(f32::MAX) * 2.0, 0.0, 0.0];
630 assert!(invalid(call(&far, points, 1.0, 4)));
631 let mut foreign = targets.clone();
632 foreign[1].bvh = &s.bvhs[0];
633 assert_eq!(
634 call(&foreign, points, 1.0, 4),
635 Err(FieldsError::Query(QueryError::MeshMismatch))
636 );
637 let mut wrong_sign = targets.clone();
638 wrong_sign[0].sign = &signs[1];
639 assert!(invalid(call(&wrong_sign, points, 1.0, 4)));
640 assert_eq!(out, [7.0; 4]);
641 assert_eq!(nearest, [7; 4]);
642
643 signed_distances_multi(&[], points, 1.0, &mut out, &mut nearest).unwrap();
645 assert_eq!(out, [f32::INFINITY; 4]);
646 assert_eq!(nearest, [u32::MAX; 4]);
647 }
648}