1use crate::navmesh::detour::common::{
29 distance_pt_poly_edges_sqr, dt_clamp, dt_max, dt_min, overlap_bounds, overlap_quant_bounds,
30 point_in_polygon, vadd, vdist, vlerp, vsub,
31};
32use crate::navmesh::detour::mesh::NavMesh;
33use crate::navmesh::detour::set::decode_poly_ref;
34use crate::navmesh::detour::tile::{Poly, TileData};
35use crate::navmesh::num::{floor_i32, trunc_u16};
36
37const POLYTYPE_OFFMESH: u8 = 1;
39const VERTS_PER_POLYGON: usize = 6;
41const NEAREST_MAX_POLYS: usize = 128;
43
44fn poly_verts(tile: &TileData, poly: usize) -> Option<([[f32; 3]; VERTS_PER_POLYGON], usize)> {
47 let p = tile.polys.get(poly)?;
48 let nv = usize::from(p.vert_count);
49 if nv > VERTS_PER_POLYGON {
50 return None;
51 }
52 let mut out = [[0.0_f32; 3]; VERTS_PER_POLYGON];
53 for (slot, &vi) in out.iter_mut().zip(&p.verts).take(nv) {
54 *slot = *tile.verts.get(usize::from(vi))?;
55 }
56 Some((out, nv))
57}
58
59#[expect(
62 clippy::many_single_char_names,
63 reason = "the names of the upstream function are kept for comparison"
64)]
65fn closest_height_point_triangle(
66 p: &[f32; 3],
67 a: &[f32; 3],
68 b: &[f32; 3],
69 c: &[f32; 3],
70) -> Option<f32> {
71 let v0 = vsub(c, a);
72 let v1 = vsub(b, a);
73 let v2 = vsub(p, a);
74 let dot00 = v0[0] * v0[0] + v0[2] * v0[2];
75 let dot01 = v0[0] * v1[0] + v0[2] * v1[2];
76 let dot02 = v0[0] * v2[0] + v0[2] * v2[2];
77 let dot11 = v1[0] * v1[0] + v1[2] * v1[2];
78 let dot12 = v1[0] * v2[0] + v1[2] * v2[2];
79 let inv_denom = 1.0 / (dot00 * dot11 - dot01 * dot01);
80 let u = (dot11 * dot02 - dot01 * dot12) * inv_denom;
81 let v = (dot00 * dot12 - dot01 * dot02) * inv_denom;
82 if u >= -1e-3 && v >= -1e-3 && u + v <= 1.001 {
83 Some((v0[1] * u + a[1]) + v1[1] * v)
84 } else {
85 None
86 }
87}
88
89fn fan_height(verts: &[[f32; 3]], pos: &[f32; 3]) -> Option<f32> {
92 let a = verts.first()?;
93 verts
94 .windows(2)
95 .skip(1)
96 .find_map(|w| closest_height_point_triangle(pos, a, w.first()?, w.get(1)?))
97}
98
99#[cfg_attr(
105 not(test),
106 expect(
107 dead_code,
108 reason = "port of getPolyHeight; the nearest-polygon query and the island filter do not need it"
109 )
110)]
111pub(crate) fn get_poly_height(tile: &TileData, poly: usize, pos: &[f32; 3]) -> Option<f32> {
112 if tile.polys.get(poly)?.poly_type() == POLYTYPE_OFFMESH {
113 return None;
114 }
115 let (arr, nv) = poly_verts(tile, poly)?;
116 let verts = arr.get(..nv)?;
117 if !point_in_polygon(pos, verts) {
118 return None;
119 }
120 if let Some(h) = fan_height(verts, pos) {
121 return Some(h);
122 }
123 let (_, closest) = clamp_to_edges(pos, verts)?;
124 Some(closest[1])
125}
126
127fn clamp_to_edges(pos: &[f32; 3], verts: &[[f32; 3]]) -> Option<(bool, [f32; 3])> {
129 let mut ed = [0.0_f32; VERTS_PER_POLYGON];
130 let mut et = [0.0_f32; VERTS_PER_POLYGON];
131 let inside = distance_pt_poly_edges_sqr(pos, verts, &mut ed, &mut et)?;
132 let mut dmin = *ed.first()?;
134 let mut imin = 0;
135 for (i, &d) in ed.iter().enumerate().take(verts.len()).skip(1) {
136 if d < dmin {
137 dmin = d;
138 imin = i;
139 }
140 }
141 let va = verts.get(imin)?;
142 let vb = verts.get((imin + 1) % verts.len())?;
143 Some((inside, vlerp(va, vb, *et.get(imin)?)))
144}
145
146pub(crate) fn closest_point_on_poly(
155 tile: &TileData,
156 poly: usize,
157 pos: &[f32; 3],
158) -> Option<([f32; 3], bool)> {
159 let p = tile.polys.get(poly)?;
160 let mut closest = *pos;
161 if p.poly_type() != POLYTYPE_OFFMESH {
162 let (arr, nv) = poly_verts(tile, poly)?;
163 let verts = arr.get(..nv)?;
164 if point_in_polygon(pos, verts) {
165 closest[1] = fan_height(verts, pos).unwrap_or(pos[1]);
167 return Some((closest, true));
168 }
169 }
170 if p.poly_type() == POLYTYPE_OFFMESH {
171 let v0 = tile.verts.get(usize::from(*p.verts.first()?))?;
172 let v1 = tile.verts.get(usize::from(*p.verts.get(1)?))?;
173 let d0 = vdist(pos, v0);
174 let d1 = vdist(pos, v1);
175 let sum = d0 + d1;
176 let u = if sum > 0.0 { d0 / sum } else { 0.0 };
177 return Some((vlerp(v0, v1, u), false));
178 }
179 let (arr, nv) = poly_verts(tile, poly)?;
180 let verts = arr.get(..nv)?;
181 let (_, mut closest) = clamp_to_edges(pos, verts)?;
182 if let Some(h) = fan_height(verts, pos) {
183 closest[1] = h;
184 }
185 Some((closest, false))
186}
187
188pub(crate) fn query_polygons_in_tile(
192 tile: &TileData,
193 base: u64,
194 qmin: &[f32; 3],
195 qmax: &[f32; 3],
196 max_polys: usize,
197) -> Vec<u64> {
198 query_polygons_filtered(tile, base, qmin, qmax, max_polys, &|_| true)
199}
200
201pub(crate) fn query_polygons_filtered(
204 tile: &TileData,
205 base: u64,
206 qmin: &[f32; 3],
207 qmax: &[f32; 3],
208 max_polys: usize,
209 filter: &dyn Fn(&Poly) -> bool,
210) -> Vec<u64> {
211 let mut out = Vec::new();
212 if tile.bv_tree.is_empty() {
213 for (i, p) in tile.polys.iter().enumerate() {
214 if p.poly_type() == POLYTYPE_OFFMESH || !filter(p) {
215 continue;
216 }
217 let Some((arr, nv)) = poly_verts(tile, i) else {
218 continue;
219 };
220 let Some((first, rest)) = arr.get(..nv).and_then(<[_]>::split_first) else {
221 continue;
222 };
223 let mut bmin = *first;
224 let mut bmax = *first;
225 for v in rest {
226 for ((lo, hi), &c) in bmin.iter_mut().zip(bmax.iter_mut()).zip(v) {
227 *lo = dt_min(*lo, c);
228 *hi = dt_max(*hi, c);
229 }
230 }
231 if overlap_bounds(qmin, qmax, &bmin, &bmax) && out.len() < max_polys {
232 out.push(base | u64::try_from(i).unwrap_or(0));
233 }
234 }
235 return out;
236 }
237
238 let tbmin = &tile.header.bmin;
239 let tbmax = &tile.header.bmax;
240 let qfac = tile.header.bv_quant_factor;
241 let quant = |v: f32, lo: f32, hi: f32| qfac * (dt_clamp(v, lo, hi) - lo);
243 let min_q = |v: f32, lo: f32, hi: f32| trunc_u16(quant(v, lo, hi)) & 0xfffe;
244 let max_q = |v: f32, lo: f32, hi: f32| trunc_u16(quant(v, lo, hi) + 1.0) | 1;
245 let bmin = [
246 min_q(qmin[0], tbmin[0], tbmax[0]),
247 min_q(qmin[1], tbmin[1], tbmax[1]),
248 min_q(qmin[2], tbmin[2], tbmax[2]),
249 ];
250 let bmax = [
251 max_q(qmax[0], tbmin[0], tbmax[0]),
252 max_q(qmax[1], tbmin[1], tbmax[1]),
253 max_q(qmax[2], tbmin[2], tbmax[2]),
254 ];
255
256 let mut at = 0_usize;
257 while let Some(node) = tile.bv_tree.get(at) {
258 let overlap = overlap_quant_bounds(bmin, bmax, node.bmin, node.bmax);
259 let leaf = node.i >= 0;
260 if leaf
261 && overlap
262 && out.len() < max_polys
263 && let Ok(i) = usize::try_from(node.i)
264 && tile
265 .polys
266 .get(i)
267 .is_some_and(|p| p.poly_type() != POLYTYPE_OFFMESH && filter(p))
268 {
269 out.push(base | u64::try_from(i).unwrap_or(0));
270 }
271 if overlap || leaf {
272 at += 1;
273 } else {
274 let Some(escape) = node.i.checked_neg().and_then(|e| usize::try_from(e).ok()) else {
276 break;
277 };
278 let Some(next) = at.checked_add(escape) else {
279 break;
280 };
281 at = next;
282 }
283 }
284 out
285}
286
287struct Nearest {
289 distance_sqr: f32,
290 found: Option<(u64, [f32; 3])>,
291}
292
293impl Nearest {
294 fn new() -> Self {
295 Self {
296 distance_sqr: f32::MAX,
297 found: None,
298 }
299 }
300
301 fn process(&mut self, tile: &TileData, refs: &[u64], center: &[f32; 3]) {
306 for &r in refs {
307 let idx = usize::try_from(decode_poly_ref(r).2).unwrap_or(usize::MAX);
308 let Some((closest, over)) = closest_point_on_poly(tile, idx, center) else {
309 continue;
310 };
311 let diff = vsub(center, &closest);
312 let d = if over {
313 let d = diff[1].abs() - tile.header.walkable_climb;
314 if d > 0.0 { d * d } else { 0.0 }
315 } else {
316 diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2]
317 };
318 if d < self.distance_sqr {
319 self.found = Some((r, closest));
320 self.distance_sqr = d;
321 }
322 }
323 }
324}
325
326pub(crate) fn find_nearest_poly_in_tile(
329 tile: &TileData,
330 base: u64,
331 center: &[f32; 3],
332 half_extents: &[f32; 3],
333) -> Option<(u64, [f32; 3])> {
334 let bmin = vsub(center, half_extents);
335 let bmax = vadd(center, half_extents);
336 let polys = query_polygons_in_tile(tile, base, &bmin, &bmax, NEAREST_MAX_POLYS);
337 let mut nearest = Nearest::new();
338 nearest.process(tile, &polys, center);
339 nearest.found
340}
341
342#[must_use]
354pub fn find_nearest_poly(
355 mesh: &NavMesh,
356 center: [f32; 3],
357 half_extents: [f32; 3],
358 include: u16,
359 exclude: u16,
360) -> Option<(u64, [f32; 3])> {
361 if !center.iter().chain(&half_extents).all(|v| v.is_finite()) {
363 return None;
364 }
365 let bmin = vsub(¢er, &half_extents);
366 let bmax = vadd(¢er, &half_extents);
367 let params = mesh.params();
368 let loc = |pos: &[f32; 3]| {
369 (
370 floor_i32((pos[0] - params.orig[0]) / params.tile_width),
371 floor_i32((pos[2] - params.orig[2]) / params.tile_height),
372 )
373 };
374 let (lo_x, lo_y) = loc(&bmin);
375 let (hi_x, hi_y) = loc(&bmax);
376 let pass = |p: &Poly| p.flags & include != 0 && p.flags & exclude == 0;
377 let mut nearest = Nearest::new();
378 for (base, tile) in mesh.tiles_in_range((lo_x, lo_y), (hi_x, hi_y)) {
379 let polys = query_polygons_filtered(tile, base, &bmin, &bmax, usize::MAX, &pass);
380 nearest.process(tile, &polys, ¢er);
381 }
382 nearest.found
383}
384
385#[cfg(test)]
386#[expect(
387 clippy::field_reassign_with_default,
388 reason = "the tile types are non_exhaustive; tests fill them field by field"
389)]
390mod tests {
391 use super::*;
392 use crate::navmesh::detour::tile::{BvNode, MeshHeader, Poly};
393
394 fn slope() -> TileData {
396 let mut quad = Poly::default();
397 quad.verts = [0, 1, 2, 3, 0, 0];
398 quad.vert_count = 4;
399 let mut off = Poly::default();
400 off.verts = [4, 5, 0, 0, 0, 0];
401 off.vert_count = 2;
402 off.set_type(1);
403 let mut t = TileData::default();
404 t.header = MeshHeader::default();
405 t.header.bmin = [0.0; 3];
406 t.header.bmax = [10.0, 10.0, 10.0];
407 t.header.bv_quant_factor = 1.0;
408 t.header.walkable_climb = 0.5;
409 t.verts = vec![
410 [0.0, 0.0, 0.0],
411 [0.0, 0.0, 10.0],
412 [10.0, 10.0, 10.0],
413 [10.0, 10.0, 0.0],
414 [20.0, 0.0, 0.0],
415 [30.0, 0.0, 0.0],
416 ];
417 t.polys = vec![quad, off];
418 t
419 }
420
421 #[test]
422 fn height_over_a_sloped_polygon() {
423 let t = slope();
424 let h = get_poly_height(&t, 0, &[5.0, 99.0, 5.0]).expect("over the polygon");
425 assert!((h - 5.0).abs() < 1e-4);
426 assert!(get_poly_height(&t, 0, &[15.0, 0.0, 5.0]).is_none());
427 assert!(get_poly_height(&t, 1, &[25.0, 0.0, 0.0]).is_none());
428 let (c, over) = closest_point_on_poly(&t, 0, &[2.0, 50.0, 8.0]).expect("poly");
429 assert!(over && (c[1] - 2.0).abs() < 1e-4 && (c[0] - 2.0).abs() < f32::EPSILON);
430 }
431
432 #[test]
433 fn inside_point_no_fan_triangle_contains_keeps_its_y() {
434 let mut t = slope();
438 t.verts = vec![
439 [0.0, 0.0, 0.0],
440 [0.0, 0.0, 1e10],
441 [1e10, 0.0, 1e10],
442 [1e10, 0.0, 0.0],
443 [0.0; 3],
444 [0.0; 3],
445 ];
446 let pos = [5e9, 77.0, 5e9];
447 assert!(fan_height(&t.verts[..4], &pos).is_none());
448 let (c, over) = closest_point_on_poly(&t, 0, &pos).expect("poly");
449 assert!(over);
450 assert!((c[1] - 77.0).abs() < f32::EPSILON);
451 let h = get_poly_height(&t, 0, &pos).expect("over the polygon");
453 assert!(h.abs() < f32::EPSILON);
454 }
455
456 #[test]
457 fn outside_points_clamp_to_the_nearest_edge() {
458 let t = slope();
459 let (c, over) = closest_point_on_poly(&t, 0, &[15.0, 0.0, 5.0]).expect("poly");
461 assert!(!over);
462 assert!((c[0] - 10.0).abs() < 1e-4 && (c[2] - 5.0).abs() < 1e-4);
463 assert!((c[1] - 10.0).abs() < 1e-4);
464 }
465
466 fn warped() -> TileData {
469 let mut t = slope();
470 t.verts[2] = [10.0, 20.0, 10.0];
471 t.verts[3] = [10.0, 0.0, 0.0];
472 t
473 }
474
475 #[test]
476 fn closest_point_inside_uses_fan_height() {
477 let t = warped();
478 let pos = [5.0, 77.0, 4.991];
481 let (c, over) = closest_point_on_poly(&t, 0, &pos).expect("poly");
482 assert!(over);
483 assert!((c[0] - 5.0).abs() < 1e-6 && (c[2] - 4.991).abs() < 1e-6);
484 assert!((c[1] - 10.0).abs() < 0.005, "height {}", c[1]);
486 let second = closest_height_point_triangle(&pos, &t.verts[0], &t.verts[2], &t.verts[3])
487 .expect("second triangle contains it");
488 assert!((second - 9.982).abs() < 0.005 && (second - c[1]).abs() > 0.01);
489 }
490
491 #[test]
492 fn closest_point_outside_clamps_to_edge() {
493 let t = slope();
494 let (c, over) = closest_point_on_poly(&t, 0, &[10.005, 0.0, 5.0]).expect("poly");
498 assert!(!over);
499 assert!((c[0] - 10.0).abs() < 1e-4 && (c[2] - 5.0).abs() < 1e-4);
500 assert!((c[1] - 10.005).abs() < 1e-3, "height {}", c[1]);
501 }
502
503 #[test]
504 fn off_mesh_polygons_interpolate_by_distance() {
505 let t = slope();
506 let (c, over) = closest_point_on_poly(&t, 1, &[22.0, 0.0, 0.0]).expect("poly");
507 assert!(!over);
508 assert!((c[0] - 22.0).abs() < 1e-4);
510 assert!(closest_point_on_poly(&t, 9, &[0.0; 3]).is_none());
511 }
512
513 #[test]
514 fn brute_force_query_skips_off_mesh_polygons() {
515 let t = slope();
516 let all = query_polygons_in_tile(&t, 7 << 20, &[-1.0; 3], &[40.0; 3], 8);
517 assert_eq!(all, vec![7 << 20]);
518 assert_eq!(
519 query_polygons_in_tile(&t, 0, &[50.0; 3], &[60.0; 3], 8).len(),
520 0
521 );
522 assert_eq!(
523 query_polygons_in_tile(&t, 0, &[-1.0; 3], &[40.0; 3], 0).len(),
524 0
525 );
526 }
527
528 #[test]
529 fn bv_tree_query_follows_escape_indices() {
530 let mut t = slope();
531 let mut root = BvNode::default();
533 root.bmin = [0; 3];
534 root.bmax = [10; 3];
535 root.i = -3;
536 let mut leaf = BvNode::default();
537 leaf.bmin = [0; 3];
538 leaf.bmax = [10; 3];
539 let mut stray = leaf.clone();
540 stray.i = 40; t.bv_tree = vec![root, leaf, stray];
542 let hits = query_polygons_in_tile(&t, 0, &[1.0; 3], &[2.0; 3], 8);
543 assert_eq!(hits, vec![0]);
544 t.header.bmax = [100.0; 3];
546 assert_eq!(
547 query_polygons_in_tile(&t, 0, &[50.0; 3], &[60.0; 3], 8).len(),
548 0
549 );
550 }
551
552 #[test]
553 fn nearest_poly_prefers_the_one_below_within_climb() {
554 let t = slope();
555 let (r, p) = find_nearest_poly_in_tile(&t, 3 << 20, &[5.0, 5.4, 5.0], &[0.0, 5.0, 0.0])
557 .expect("found");
558 assert_eq!(r, 3 << 20);
559 assert!((p[1] - 5.0).abs() < 1e-4);
560 assert!(find_nearest_poly_in_tile(&t, 0, &[50.0, 0.0, 5.0], &[1.0; 3]).is_none());
561 }
562}