1use std::collections::BTreeMap;
30use std::io;
31
32use crate::error::ErrorExt;
33use crate::navmesh::detour::common::{dt_clamp, dt_max, dt_min, opposite_tile};
34use crate::navmesh::detour::query::find_nearest_poly_in_tile;
35use crate::navmesh::detour::set::{
36 NavMeshParams, NavMeshSet, SET_VERSION, SetTile, decode_poly_ref, encode_poly_ref,
37};
38use crate::navmesh::detour::tile::{NAVMESH_MAGIC, NAVMESH_VERSION, Poly, TileData};
39use crate::navmesh::num::trunc_u8;
40
41const NULL_LINK: u32 = 0xffff_ffff;
43const EXT_LINK: u16 = 0x8000;
45const POLYTYPE_OFFMESH: u8 = 1;
47const OFFMESH_CON_BIDIR: u8 = 1;
49const MAX_POLYS_PER_TILE: usize = 1 << 20;
51const MAX_TILE_SLOTS: u32 = 1 << 28;
53const SALT_MASK: u32 = 0xffff;
55const MAX_CONNECTING: usize = 4;
57const OBSTACLE_SNAP_HALF_HEIGHT: f32 = 30.0;
59
60#[derive(Debug, Clone)]
62struct MeshTile {
63 data: TileData,
64 links_free_list: u32,
65 padding: u32,
67}
68
69#[derive(Debug, Clone)]
71struct Slot {
72 salt: u32,
73 tile: Option<MeshTile>,
74}
75
76#[derive(Debug, Clone)]
83pub struct NavMesh {
84 params: NavMeshParams,
85 max_tiles: u32,
86 slots: BTreeMap<u32, Slot>,
88 next_fresh: u32,
90 free: Vec<u32>,
92 grid: BTreeMap<(i32, i32), u32>,
94}
95
96fn salt_after_removal(salt: u32) -> u32 {
97 let s = salt.wrapping_add(1) & SALT_MASK;
98 if s == 0 { 1 } else { s }
99}
100
101fn alloc_link(t: &mut MeshTile) -> u32 {
102 if t.links_free_list == NULL_LINK {
103 return NULL_LINK;
104 }
105 let link = t.links_free_list;
106 let next = usize::try_from(link)
107 .ok()
108 .and_then(|i| t.data.links.get(i))
109 .map_or(NULL_LINK, |l| l.next);
110 t.links_free_list = next;
111 link
112}
113
114fn free_link(t: &mut MeshTile, link: u32) {
115 let head = t.links_free_list;
116 if let Some(l) = usize::try_from(link)
117 .ok()
118 .and_then(|i| t.data.links.get_mut(i))
119 {
120 l.next = head;
121 t.links_free_list = link;
122 }
123}
124
125fn push_link(
129 t: &mut MeshTile,
130 poly: usize,
131 reference: u64,
132 edge: u8,
133 side: u8,
134 bounds: Option<(u8, u8)>,
135) -> bool {
136 let idx = alloc_link(t);
137 if idx == NULL_LINK {
138 return false;
139 }
140 let Some(first) = t.data.polys.get(poly).map(|p| p.first_link) else {
141 free_link(t, idx);
142 return false;
143 };
144 let linked = usize::try_from(idx)
145 .ok()
146 .and_then(|i| t.data.links.get_mut(i));
147 if let Some(l) = linked {
148 l.reference = reference;
149 l.edge = edge;
150 l.side = side;
151 if let Some((lo, hi)) = bounds {
152 l.bmin = lo;
153 l.bmax = hi;
154 }
155 l.next = first;
156 }
157 if let Some(p) = t.data.polys.get_mut(poly) {
158 p.first_link = idx;
159 }
160 true
161}
162
163fn vert(t: &TileData, index: u16) -> Option<[f32; 3]> {
164 t.verts.get(usize::from(index)).copied()
165}
166
167fn sqr(x: f32) -> f32 {
168 x * x
169}
170
171fn get_slab_coord(va: &[f32; 3], side: i32) -> f32 {
173 if side == 0 || side == 4 {
174 va[0]
175 } else if side == 2 || side == 6 {
176 va[2]
177 } else {
178 0.0
179 }
180}
181
182fn calc_slab_end_points(va: &[f32; 3], vb: &[f32; 3], side: i32) -> ([f32; 2], [f32; 2]) {
184 let axis = if side == 0 || side == 4 {
185 2
186 } else if side == 2 || side == 6 {
187 0
188 } else {
189 return ([0.0; 2], [0.0; 2]);
190 };
191 let (Some(&a), Some(&b)) = (va.get(axis), vb.get(axis)) else {
192 return ([0.0; 2], [0.0; 2]);
193 };
194 if a < b {
195 ([a, va[1]], [b, vb[1]])
196 } else {
197 ([b, vb[1]], [a, va[1]])
198 }
199}
200
201fn overlap_slabs(
203 amin: [f32; 2],
204 amax: [f32; 2],
205 bmin: [f32; 2],
206 bmax: [f32; 2],
207 px: f32,
208 py: f32,
209) -> bool {
210 let lo_x = dt_max(amin[0] + px, bmin[0] + px);
212 let hi_x = dt_min(amax[0] - px, bmax[0] - px);
213 if lo_x > hi_x {
214 return false;
215 }
216 let ad = (amax[1] - amin[1]) / (amax[0] - amin[0]);
217 let ak = amin[1] - ad * amin[0];
218 let bd = (bmax[1] - bmin[1]) / (bmax[0] - bmin[0]);
219 let bk = bmin[1] - bd * bmin[0];
220 let aminy = ad * lo_x + ak;
221 let amaxy = ad * hi_x + ak;
222 let bminy = bd * lo_x + bk;
223 let bmaxy = bd * hi_x + bk;
224 let dmin = bminy - aminy;
225 let dmax = bmaxy - amaxy;
226 if dmin * dmax < 0.0 {
228 return true;
229 }
230 let thr = sqr(py * 2.0);
232 dmin * dmin <= thr || dmax * dmax <= thr
233}
234
235struct Connection {
237 reference: u64,
238 lo: f32,
239 hi: f32,
240}
241
242fn find_connecting_polys(
245 va: &[f32; 3],
246 vb: &[f32; 3],
247 tile: &TileData,
248 base: u64,
249 side: i32,
250 max: usize,
251) -> Vec<Connection> {
252 let mut out = Vec::new();
253 let (amin, amax) = calc_slab_end_points(va, vb, side);
254 let apos = get_slab_coord(va, side);
255 let Ok(side_bits) = u16::try_from(side) else {
256 return out;
257 };
258 let m = EXT_LINK | side_bits;
259 for (i, poly) in tile.polys.iter().enumerate() {
260 let nv = usize::from(poly.vert_count);
261 for j in 0..nv {
262 if poly.neis.get(j).copied() != Some(m) {
264 continue;
265 }
266 let k = (j + 1) % nv;
267 let (Some(vc), Some(vd)) = (
268 poly.verts.get(j).and_then(|&v| vert(tile, v)),
269 poly.verts.get(k).and_then(|&v| vert(tile, v)),
270 ) else {
271 continue;
272 };
273 let bpos = get_slab_coord(&vc, side);
274 if (apos - bpos).abs() > 0.01 {
276 continue;
277 }
278 let (bmin, bmax) = calc_slab_end_points(&vc, &vd, side);
280 if !overlap_slabs(amin, amax, bmin, bmax, 0.01, tile.header.walkable_climb) {
281 continue;
282 }
283 if out.len() < max {
284 out.push(Connection {
285 reference: base | u64::try_from(i).unwrap_or(0),
286 lo: dt_max(amin[0], bmin[0]),
287 hi: dt_min(amax[0], bmax[0]),
288 });
289 }
290 break;
291 }
292 }
293 out
294}
295
296fn portal_limits(lo: f32, hi: f32, start: f32, end: f32) -> (u8, u8) {
299 let mut tmin = (lo - start) / (end - start);
300 let mut tmax = (hi - start) / (end - start);
301 if tmin > tmax {
302 std::mem::swap(&mut tmin, &mut tmax);
303 }
304 (
305 trunc_u8(dt_clamp(tmin, 0.0, 1.0) * 255.0),
306 trunc_u8(dt_clamp(tmax, 0.0, 1.0) * 255.0),
307 )
308}
309
310struct PortalLink {
312 poly: usize,
313 edge: u8,
314 side: u8,
315 reference: u64,
316 bounds: Option<(u8, u8)>,
317}
318
319fn plan_ext_links(
321 tile: &TileData,
322 target: &TileData,
323 target_base: u64,
324 side: i32,
325) -> Vec<PortalLink> {
326 let mut plan = Vec::new();
327 for (i, poly) in tile.polys.iter().enumerate() {
328 let nv = usize::from(poly.vert_count);
329 for j in 0..nv {
330 let Some(&nei) = poly.neis.get(j) else {
331 continue;
332 };
333 if nei & EXT_LINK == 0 {
335 continue;
336 }
337 let dir = i32::from(nei & 0xff);
338 if side != -1 && dir != side {
339 continue;
340 }
341 let (Some(va), Some(vb)) = (
342 poly.verts.get(j).and_then(|&v| vert(tile, v)),
343 poly.verts.get((j + 1) % nv).and_then(|&v| vert(tile, v)),
344 ) else {
345 continue;
346 };
347 let found = find_connecting_polys(
348 &va,
349 &vb,
350 target,
351 target_base,
352 opposite_tile(dir),
353 MAX_CONNECTING,
354 );
355 for c in found {
356 let bounds = if dir == 0 || dir == 4 {
358 Some(portal_limits(c.lo, c.hi, va[2], vb[2]))
359 } else if dir == 2 || dir == 6 {
360 Some(portal_limits(c.lo, c.hi, va[0], vb[0]))
361 } else {
362 None
363 };
364 plan.push(PortalLink {
365 poly: i,
366 edge: u8::try_from(j).unwrap_or(0),
367 side: u8::try_from(nei & 0xff).unwrap_or(0),
368 reference: c.reference,
369 bounds,
370 });
371 }
372 }
373 }
374 plan
375}
376
377struct OffMeshLanding {
379 con_poly: usize,
381 vert: usize,
383 nearest: [f32; 3],
385 reference: u64,
387 bidirectional: bool,
388}
389
390fn plan_ext_off_mesh_links(
392 tile: &TileData,
393 tile_base: u64,
394 target: &TileData,
395 opposite_side: u8,
396) -> Vec<OffMeshLanding> {
397 let mut plan = Vec::new();
398 for con in &target.off_mesh {
400 if con.side != opposite_side {
401 continue;
402 }
403 let con_poly = usize::from(con.poly);
404 let Some(target_poly) = target.polys.get(con_poly) else {
405 continue;
406 };
407 if target_poly.first_link == NULL_LINK {
409 continue;
410 }
411 let Some(&vert_index) = target_poly.verts.get(1) else {
412 continue;
413 };
414 let half_extents = [con.rad, target.header.walkable_climb, con.rad];
415 let p = [con.pos[3], con.pos[4], con.pos[5]];
417 let Some((reference, nearest)) =
418 find_nearest_poly_in_tile(tile, tile_base, &p, &half_extents)
419 else {
420 continue;
421 };
422 if sqr(nearest[0] - p[0]) + sqr(nearest[2] - p[2]) > sqr(con.rad) {
424 continue;
425 }
426 plan.push(OffMeshLanding {
427 con_poly,
428 vert: usize::from(vert_index),
429 nearest,
430 reference,
431 bidirectional: con.flags & OFFMESH_CON_BIDIR != 0,
432 });
433 }
434 plan
435}
436
437fn connect_int_links(t: &mut MeshTile, base: u64) {
439 for i in 0..t.data.polys.len() {
440 let Some(poly) = t.data.polys.get_mut(i) else {
441 break;
442 };
443 poly.first_link = NULL_LINK;
444 if poly.poly_type() == POLYTYPE_OFFMESH {
445 continue;
446 }
447 let neis = poly.neis;
448 let nv = usize::from(poly.vert_count);
449 for j in (0..nv).rev() {
452 let nei = neis.get(j).copied().unwrap_or(0);
453 if nei == 0 || nei & EXT_LINK != 0 {
455 continue;
456 }
457 let target = u64::from(nei - 1);
458 push_link(
459 t,
460 i,
461 base | target,
462 u8::try_from(j).unwrap_or(0),
463 0xff,
464 Some((0, 0)),
465 );
466 }
467 }
468}
469
470fn base_off_mesh_links(t: &mut MeshTile, base: u64) {
475 for i in 0..t.data.off_mesh.len() {
477 let Some(con) = t.data.off_mesh.get_mut(i) else {
478 break;
479 };
480 con.reserved = 1.0;
481 let (pos, rad, con_poly) = (con.pos, con.rad, usize::from(con.poly));
482 let half_extents = [rad, t.data.header.walkable_climb, rad];
483 let p = [pos[0], pos[1], pos[2]];
485 let Some((reference, nearest)) =
486 find_nearest_poly_in_tile(&t.data, base, &p, &half_extents)
487 else {
488 continue;
489 };
490 if sqr(nearest[0] - p[0]) + sqr(nearest[2] - p[2]) > sqr(rad) {
492 continue;
493 }
494 let first = t
496 .data
497 .polys
498 .get(con_poly)
499 .and_then(|poly| poly.verts.first().copied());
500 let Some(v) = first.and_then(|v| t.data.verts.get_mut(usize::from(v))) else {
501 continue;
502 };
503 *v = nearest;
504
505 if push_link(t, con_poly, reference, 0, 0xff, Some((0, 0))) {
507 let land = usize::try_from(decode_poly_ref(reference).2).unwrap_or(usize::MAX);
509 push_link(
510 t,
511 land,
512 base | u64::try_from(con_poly).unwrap_or(0),
513 0xff,
514 0xff,
515 Some((0, 0)),
516 );
517 }
518 }
519}
520
521impl NavMesh {
522 pub fn new(params: NavMeshParams) -> io::Result<Self> {
530 let max_tiles = u32::try_from(params.max_tiles)
531 .ok()
532 .filter(|&n| n > 0 && n <= MAX_TILE_SLOTS)
533 .ok_or_else(|| error!("invalid maxTiles {}", params.max_tiles))?;
534 Ok(Self {
535 params,
536 max_tiles,
537 slots: BTreeMap::new(),
538 next_fresh: 0,
539 free: Vec::new(),
540 grid: BTreeMap::new(),
541 })
542 }
543
544 pub fn from_set(set: &NavMeshSet) -> io::Result<Self> {
553 let mut mesh = Self::new(set.params.clone())?;
554 for (i, t) in set.tiles.iter().enumerate() {
555 mesh.add_tile(t.data.clone(), t.tile_ref, t.padding)
556 .prepend_error(format!("tile record {i}:"))?;
557 }
558 Ok(mesh)
559 }
560
561 #[must_use]
563 pub fn params(&self) -> &NavMeshParams {
564 &self.params
565 }
566
567 pub fn set_tile(&mut self, data: TileData) -> io::Result<u64> {
575 Self::validate_tile(&data)?;
576 let key = (data.header.x, data.header.y);
577 let last_ref = match self.grid.get(&key).copied() {
578 Some(slot) => {
579 self.remove_slot(slot);
580 encode_poly_ref(self.salt_of(slot), slot, 0)
581 }
582 None => 0,
583 };
584 self.add_tile(data, last_ref, 0)
585 }
586
587 pub fn remove_tile_at(&mut self, x: i32, y: i32) -> bool {
590 match self.grid.get(&(x, y)).copied() {
591 Some(slot) => {
592 self.remove_slot(slot);
593 true
594 }
595 None => false,
596 }
597 }
598
599 #[must_use]
601 pub fn tile_at(&self, x: i32, y: i32) -> Option<&TileData> {
602 let slot = self.grid.get(&(x, y))?;
603 self.tile(*slot).map(|t| &t.data)
604 }
605
606 pub fn tiles(&self) -> impl Iterator<Item = (u64, &TileData)> {
608 self.slots.iter().filter_map(|(&slot, s)| {
609 s.tile
610 .as_ref()
611 .map(|t| (encode_poly_ref(s.salt, slot, 0), &t.data))
612 })
613 }
614
615 #[must_use]
618 pub fn to_set(&self, tiles_x: i32, tiles_z: i32) -> NavMeshSet {
619 NavMeshSet {
620 version: SET_VERSION,
621 tiles_x,
622 tiles_z,
623 params: self.params.clone(),
624 tiles: self
625 .slots
626 .iter()
627 .filter_map(|(&slot, s)| {
628 s.tile.as_ref().map(|t| SetTile {
629 tile_ref: encode_poly_ref(s.salt, slot, 0),
630 padding: t.padding,
631 data: t.data.clone(),
632 })
633 })
634 .collect(),
635 }
636 }
637
638 #[must_use]
641 pub fn into_set(self, tiles_x: i32, tiles_z: i32) -> NavMeshSet {
642 NavMeshSet {
643 version: SET_VERSION,
644 tiles_x,
645 tiles_z,
646 params: self.params,
647 tiles: self
648 .slots
649 .into_iter()
650 .filter_map(|(slot, s)| {
651 let salt = s.salt;
652 s.tile.map(|t| SetTile {
653 tile_ref: encode_poly_ref(salt, slot, 0),
654 padding: t.padding,
655 data: t.data,
656 })
657 })
658 .collect(),
659 }
660 }
661
662 pub(crate) fn tiles_in_range(
665 &self,
666 (lo_x, lo_y): (i32, i32),
667 (hi_x, hi_y): (i32, i32),
668 ) -> Vec<(u64, &TileData)> {
669 if lo_x > hi_x || lo_y > hi_y {
670 return Vec::new();
671 }
672 let cells = (i64::from(hi_x) - i64::from(lo_x) + 1)
674 .saturating_mul(i64::from(hi_y) - i64::from(lo_y) + 1);
675 let mut keys: Vec<(i32, i32)> =
676 if usize::try_from(cells).is_ok_and(|c| c <= self.grid.len()) {
677 (lo_y..=hi_y)
678 .flat_map(|y| (lo_x..=hi_x).map(move |x| (x, y)))
679 .filter(|k| self.grid.contains_key(k))
680 .collect()
681 } else {
682 self.grid
684 .keys()
685 .copied()
686 .filter(|&(x, y)| (lo_x..=hi_x).contains(&x) && (lo_y..=hi_y).contains(&y))
687 .collect()
688 };
689 keys.sort_by_key(|&(x, y)| (y, x));
690 keys.iter()
691 .filter_map(|k| {
692 let slot = *self.grid.get(k)?;
693 Some((self.base_of(slot), &self.tile(slot)?.data))
694 })
695 .collect()
696 }
697
698 pub(crate) fn polys_mut(&mut self) -> impl Iterator<Item = (u32, &mut [Poly])> {
700 self.slots
701 .iter_mut()
702 .filter_map(|(&slot, s)| s.tile.as_mut().map(|t| (slot, t.data.polys.as_mut_slice())))
703 }
704
705 pub(crate) fn tile_by_ref(&self, reference: u64) -> Option<(u32, &TileData)> {
708 let (salt, slot, poly) = decode_poly_ref(reference);
709 let s = self.slots.get(&slot)?;
710 let t = s.tile.as_ref()?;
711 let valid = s.salt == salt && usize::try_from(poly).is_ok_and(|p| p < t.data.polys.len());
712 valid.then_some((slot, &t.data))
713 }
714
715 fn tile(&self, slot: u32) -> Option<&MeshTile> {
716 self.slots.get(&slot)?.tile.as_ref()
717 }
718
719 fn tile_mut(&mut self, slot: u32) -> Option<&mut MeshTile> {
720 self.slots.get_mut(&slot)?.tile.as_mut()
721 }
722
723 fn salt_of(&self, slot: u32) -> u32 {
724 self.slots.get(&slot).map_or(1, |s| s.salt)
725 }
726
727 fn base_of(&self, slot: u32) -> u64 {
728 encode_poly_ref(self.salt_of(slot), slot, 0)
729 }
730
731 fn validate_tile(d: &TileData) -> io::Result<()> {
733 let h = &d.header;
734 if h.magic != NAVMESH_MAGIC {
735 return Err(error!("tile has the wrong magic"));
736 }
737 if h.version != NAVMESH_VERSION {
738 return Err(error!(
739 "tile has version {}, not {NAVMESH_VERSION}",
740 h.version
741 ));
742 }
743 if h.layer != 0 {
744 return Err(error!("tile layer {} (only layer 0 is supported)", h.layer));
745 }
746 let counts = [
747 ("polygon", h.poly_count, d.polys.len()),
748 ("vertex", h.vert_count, d.verts.len()),
749 ("link", h.max_link_count, d.links.len()),
750 ("BV node", h.bv_node_count, d.bv_tree.len()),
751 (
752 "off-mesh connection",
753 h.off_mesh_con_count,
754 d.off_mesh.len(),
755 ),
756 ];
757 for (what, header, actual) in counts {
758 if usize::try_from(header).ok() != Some(actual) {
759 return Err(error!(
760 "header says {header} {what}s, the tile has {actual}"
761 ));
762 }
763 }
764 if d.polys.len() > MAX_POLYS_PER_TILE {
765 return Err(error!(
766 "{} polygons do not fit a polygon reference",
767 d.polys.len()
768 ));
769 }
770 for (i, p) in d.polys.iter().enumerate() {
771 let nv = usize::from(p.vert_count);
772 if nv > p.verts.len() {
773 return Err(error!("polygon {i} has {nv} vertices"));
774 }
775 for (&v, &nei) in p.verts.iter().zip(&p.neis).take(nv) {
776 if usize::from(v) >= d.verts.len() {
777 return Err(error!("polygon {i} uses vertex {v} outside the tile"));
778 }
779 if nei != 0 && nei & EXT_LINK == 0 && usize::from(nei) > d.polys.len() {
780 return Err(error!("polygon {i} has neighbour {nei} outside the tile"));
781 }
782 }
783 }
784 for (i, c) in d.off_mesh.iter().enumerate() {
785 let ok = d
786 .polys
787 .get(usize::from(c.poly))
788 .is_some_and(|p| p.vert_count >= 2);
789 if !ok {
790 return Err(error!(
791 "off-mesh connection {i} has no polygon with two vertices"
792 ));
793 }
794 }
795 Ok(())
796 }
797
798 fn add_tile(&mut self, mut data: TileData, last_ref: u64, padding: u32) -> io::Result<u64> {
800 Self::validate_tile(&data)?;
801 let key = (data.header.x, data.header.y);
802 if self.grid.contains_key(&key) {
803 return Err(error!(
804 "tile cell ({}, {}) is already occupied",
805 key.0, key.1
806 ));
807 }
808 let (slot, salt) = if last_ref == 0 {
809 self.alloc_next()?
810 } else {
811 self.alloc_specific(last_ref)?
812 };
813
814 let n = data.links.len();
816 for (i, l) in data.links.iter_mut().enumerate() {
817 l.next = if i + 1 < n {
818 u32::try_from(i + 1).unwrap_or(NULL_LINK)
819 } else {
820 NULL_LINK
821 };
822 }
823 let links_free_list = if n == 0 { NULL_LINK } else { 0 };
824 self.slots.insert(
825 slot,
826 Slot {
827 salt,
828 tile: Some(MeshTile {
829 data,
830 links_free_list,
831 padding,
832 }),
833 },
834 );
835 self.grid.insert(key, slot);
836 self.link_tile(slot, key);
837 Ok(encode_poly_ref(salt, slot, 0))
838 }
839
840 fn alloc_next(&mut self) -> io::Result<(u32, u32)> {
842 if let Some(slot) = self.free.pop() {
843 return Ok((slot, self.salt_of(slot)));
844 }
845 while self.slots.contains_key(&self.next_fresh) {
846 self.next_fresh = self.next_fresh.saturating_add(1);
847 }
848 if self.next_fresh >= self.max_tiles {
849 return Err(error!("no free tile slot (maxTiles {})", self.max_tiles));
850 }
851 let slot = self.next_fresh;
852 self.next_fresh = self.next_fresh.saturating_add(1);
853 Ok((slot, 1))
854 }
855
856 fn alloc_specific(&mut self, last_ref: u64) -> io::Result<(u32, u32)> {
858 let (salt, slot, _) = decode_poly_ref(last_ref);
859 if slot >= self.max_tiles {
860 return Err(error!(
861 "tile slot {slot} is not below maxTiles {}",
862 self.max_tiles
863 ));
864 }
865 if salt == 0 {
866 return Err(error!("tile reference {last_ref:#x} has salt 0"));
867 }
868 if self.tile(slot).is_some() {
869 return Err(error!("tile slot {slot} is already in use"));
870 }
871 self.free.retain(|&s| s != slot);
872 Ok((slot, salt))
873 }
874
875 fn link_tile(&mut self, slot: u32, (x, y): (i32, i32)) {
877 let base = self.base_of(slot);
878 if let Some(t) = self.tile_mut(slot) {
879 connect_int_links(t, base);
880 base_off_mesh_links(t, base);
883 }
884 self.connect_ext_off_mesh_links(slot, slot, -1);
885
886 for side in 0..8 {
888 let Some(nei) = self.neighbour_slot(x, y, side) else {
889 continue;
890 };
891 self.connect_ext_links(slot, nei, side);
892 self.connect_ext_links(nei, slot, opposite_tile(side));
893 self.connect_ext_off_mesh_links(slot, nei, side);
894 self.connect_ext_off_mesh_links(nei, slot, opposite_tile(side));
895 }
896
897 self.snap_obstacles(slot);
898 }
899
900 fn neighbour_slot(&self, x: i32, y: i32, side: i32) -> Option<u32> {
902 let (dx, dy) = match side {
903 0 => (1, 0),
904 1 => (1, 1),
905 2 => (0, 1),
906 3 => (-1, 1),
907 4 => (-1, 0),
908 5 => (-1, -1),
909 6 => (0, -1),
910 7 => (1, -1),
911 _ => return None,
912 };
913 let key = (x.checked_add(dx)?, y.checked_add(dy)?);
914 self.grid.get(&key).copied()
915 }
916
917 fn connect_ext_links(&mut self, tile: u32, target: u32, side: i32) {
919 let plan = match (self.tile(tile), self.tile(target)) {
920 (Some(t), Some(tg)) => plan_ext_links(&t.data, &tg.data, self.base_of(target), side),
921 _ => return,
922 };
923 let Some(t) = self.tile_mut(tile) else {
924 return;
925 };
926 for l in plan {
927 push_link(t, l.poly, l.reference, l.edge, l.side, l.bounds);
928 }
929 }
930
931 fn connect_ext_off_mesh_links(&mut self, tile: u32, target: u32, side: i32) {
933 let opposite_side = if side == -1 {
934 0xff
935 } else {
936 u8::try_from(opposite_tile(side)).unwrap_or(0xff)
937 };
938 let tile_base = self.base_of(tile);
939 let target_base = self.base_of(target);
940 let plan = match (self.tile(tile), self.tile(target)) {
941 (Some(t), Some(tg)) => {
942 plan_ext_off_mesh_links(&t.data, tile_base, &tg.data, opposite_side)
943 }
944 _ => return,
945 };
946 for l in plan {
947 if let Some(tg) = self.tile_mut(target) {
949 if let Some(v) = tg.data.verts.get_mut(l.vert) {
951 *v = l.nearest;
952 }
953 push_link(tg, l.con_poly, l.reference, 1, opposite_side, Some((0, 0)));
954 }
955 if l.bidirectional
957 && let Some(t) = self.tile_mut(tile)
958 {
959 let land = usize::try_from(decode_poly_ref(l.reference).2).unwrap_or(usize::MAX);
960 let back_side = if side == -1 {
961 0xff
962 } else {
963 u8::try_from(side).unwrap_or(0xff)
964 };
965 push_link(
966 t,
967 land,
968 target_base | u64::try_from(l.con_poly).unwrap_or(0),
969 0xff,
970 back_side,
971 Some((0, 0)),
972 );
973 }
974 }
975 }
976
977 fn snap_obstacles(&mut self, slot: u32) {
981 let base = self.base_of(slot);
982 let Some(t) = self.tile_mut(slot) else {
983 return;
984 };
985 let positions: Vec<[f32; 3]> = t
986 .data
987 .extra
988 .iter()
989 .flat_map(|e| e.obstacles.iter().map(|o| o.pos))
990 .collect();
991 let half_extents = [0.0, OBSTACLE_SNAP_HALF_HEIGHT, 0.0];
992 let snapped: Vec<Option<f32>> = positions
993 .iter()
994 .map(|p| {
995 find_nearest_poly_in_tile(&t.data, base, p, &half_extents).map(|(_, pt)| pt[1])
996 })
997 .collect();
998 let obstacles = t.data.extra.iter_mut().flat_map(|e| e.obstacles.iter_mut());
999 for (o, y) in obstacles.zip(snapped) {
1000 if let Some(y) = y {
1001 o.pos[1] = y;
1002 }
1003 }
1004 }
1005
1006 fn remove_slot(&mut self, slot: u32) {
1009 let Some((x, y)) = self.tile(slot).map(|t| (t.data.header.x, t.data.header.y)) else {
1010 return;
1011 };
1012 self.grid.remove(&(x, y));
1013 for side in 0..8 {
1014 if let Some(nei) = self.neighbour_slot(x, y, side) {
1015 self.unconnect_links(nei, slot);
1016 }
1017 }
1018 if let Some(s) = self.slots.get_mut(&slot) {
1019 s.tile = None;
1020 s.salt = salt_after_removal(s.salt);
1021 }
1022 self.free.push(slot);
1023 }
1024
1025 fn unconnect_links(&mut self, tile: u32, target: u32) {
1027 let Some(t) = self.tile_mut(tile) else {
1028 return;
1029 };
1030 let polys = t.data.polys.len();
1031 for i in 0..polys {
1032 let mut j = t.data.polys.get(i).map_or(NULL_LINK, |p| p.first_link);
1033 let mut pj = NULL_LINK;
1034 let mut steps = t.data.links.len();
1036 while j != NULL_LINK && steps > 0 {
1037 steps -= 1;
1038 let Some(link) = usize::try_from(j).ok().and_then(|k| t.data.links.get(k)) else {
1039 break;
1040 };
1041 let nj = link.next;
1042 if decode_poly_ref(link.reference).1 == target {
1043 if pj == NULL_LINK {
1045 if let Some(p) = t.data.polys.get_mut(i) {
1046 p.first_link = nj;
1047 }
1048 } else if let Some(prev) = usize::try_from(pj)
1049 .ok()
1050 .and_then(|k| t.data.links.get_mut(k))
1051 {
1052 prev.next = nj;
1053 }
1054 free_link(t, j);
1055 j = nj;
1056 } else {
1057 pj = j;
1059 j = nj;
1060 }
1061 }
1062 }
1063 }
1064}