Skip to main content

dayzmake_core/navmesh/detour/
mesh.rs

1//
2// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
3//
4// This software is provided 'as-is', without any express or implied
5// warranty.  In no event will the authors be held liable for any damages
6// arising from the use of this software.
7// Permission is granted to anyone to use this software for any purpose,
8// including commercial applications, and to alter it and redistribute it
9// freely, subject to the following restrictions:
10// 1. The origin of this software must not be misrepresented; you must not
11//    claim that you wrote the original software. If you use this software
12//    in a product, an acknowledgment in the product documentation would be
13//    appreciated but is not required.
14// 2. Altered source versions must be plainly marked as such, and must not be
15//    misrepresented as being the original software.
16// 3. This notice may not be removed or altered from any source distribution.
17//
18// Ported to Rust and altered for DayZ (see the dayzmake navmesh spec); this is not the original software.
19
20//! The tiled Detour navmesh: `dtNavMesh::init`, `addTile` and `removeTile` with the tile linking
21//! (spec 8.1 to 8.3 and 10.2).
22//!
23//! Safe Rust: every tile owns its [`TileData`], whose `links` are the link pool of the tile (with a
24//! free list through the `next` fields, as upstream). Linking two tiles first reads both to
25//! compute the links and then applies them one tile at a time.
26//!
27//! DayZ differences from stock Detour (spec 10.2) are marked `DAYZ:`.
28
29use 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
41/// `DT_NULL_LINK`.
42const NULL_LINK: u32 = 0xffff_ffff;
43/// `DT_EXT_LINK`.
44const EXT_LINK: u16 = 0x8000;
45/// `DT_POLYTYPE_OFFMESH_CONNECTION`.
46const POLYTYPE_OFFMESH: u8 = 1;
47/// `DT_OFFMESH_CON_BIDIR`.
48const OFFMESH_CON_BIDIR: u8 = 1;
49/// The most polygons the 20-bit polygon field of a reference can address.
50const MAX_POLYS_PER_TILE: usize = 1 << 20;
51/// The most tile slots the 28-bit tile field of a reference can address.
52const MAX_TILE_SLOTS: u32 = 1 << 28;
53/// The salt is 16 bits wide.
54const SALT_MASK: u32 = 0xffff;
55/// The most polygons `findConnectingPolys` returns.
56const MAX_CONNECTING: usize = 4;
57/// The vertical half extent of the obstacle snap (spec 8.3).
58const OBSTACLE_SNAP_HALF_HEIGHT: f32 = 30.0;
59
60/// A loaded tile: its data and the head of its free link list.
61#[derive(Debug, Clone)]
62struct MeshTile {
63    data: TileData,
64    links_free_list: u32,
65    /// The padding word of the set record, kept on rewrite.
66    padding: u32,
67}
68
69/// One tile slot. A slot that was never touched has salt 1 and no tile.
70#[derive(Debug, Clone)]
71struct Slot {
72    salt: u32,
73    tile: Option<MeshTile>,
74}
75
76/// A tiled Detour navigation mesh (`dtNavMesh`) with 64-bit polygon references: salt 16 bits, tile
77/// 28 bits, polygon 20 bits (spec 8.1).
78///
79/// Tiles live in slots. All slots start with salt 1 and slot 0 is allocated first; removing a tile
80/// bumps the salt of its slot and puts the slot at the head of the free list. There is one layer
81/// (0) per grid cell.
82#[derive(Debug, Clone)]
83pub struct NavMesh {
84    params: NavMeshParams,
85    max_tiles: u32,
86    /// The slots that were ever used, in slot order.
87    slots: BTreeMap<u32, Slot>,
88    /// Slots that were never used are allocated in ascending order from here.
89    next_fresh: u32,
90    /// Freed slots; the head of the free list is the last element.
91    free: Vec<u32>,
92    /// Tile grid `(x, y)` to slot.
93    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
125/// Allocates a link and puts it at the head of the list of polygon `poly`. `bounds` is the portal
126/// range; `None` leaves the link's old `bmin`/`bmax` alone (upstream does not write them for the
127/// sides it does not know). Returns whether a link was available.
128fn 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
171/// `getSlabCoord`.
172fn 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
182/// `calcSlabEndPoints`: the `(min, max)` ends of the edge as (position along the slab, height).
183fn 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
201/// `overlapSlabs`.
202fn 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    // The segments are shrunk a little so that slabs which touch at end points are not connected.
211    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    // Crossing segments always overlap.
227    if dmin * dmax < 0.0 {
228        return true;
229    }
230    // Check for overlap at the end points.
231    let thr = sqr(py * 2.0);
232    dmin * dmin <= thr || dmax * dmax <= thr
233}
234
235/// A polygon of the target tile that an edge connects to, and the overlap along the slab.
236struct Connection {
237    reference: u64,
238    lo: f32,
239    hi: f32,
240}
241
242/// `findConnectingPolys`: the polygons of `tile` (whose polygon references start at `base`) with
243/// an edge on `side` that overlaps the edge `va`-`vb`, at most `max` of them.
244fn 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            // Skip edges which do not point to the right side.
263            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            // Segments are not close enough.
275            if (apos - bpos).abs() > 0.01 {
276                continue;
277            }
278            // Check if the segments touch.
279            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
296/// DAYZ: the portal limits are `(unsigned char)(clamp(t, 0, 1) * 255.0f)`, truncated; stock Detour
297/// rounds with `roundf`.
298fn 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
310/// A link `connectExtLinks` is going to add to polygon `poly`.
311struct PortalLink {
312    poly: usize,
313    edge: u8,
314    side: u8,
315    reference: u64,
316    bounds: Option<(u8, u8)>,
317}
318
319/// The links `connectExtLinks(tile, target, side)` adds, in order.
320fn 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            // Skip non-portal edges.
334            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                // Compress the portal limits to a byte value.
357                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
377/// An off-mesh connection of the target tile that lands in the tile.
378struct OffMeshLanding {
379    /// The off-mesh polygon in the target tile.
380    con_poly: usize,
381    /// The vertex of that polygon that is the landing point.
382    vert: usize,
383    /// The point on the mesh of the tile.
384    nearest: [f32; 3],
385    /// The polygon of the tile it lands on.
386    reference: u64,
387    bidirectional: bool,
388}
389
390/// The landings `connectExtOffMeshLinks(tile, target, side)` makes, in order.
391fn 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    // We are interested in the links which land from the target tile in this tile.
399    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        // Skip off-mesh connections whose start location could not be connected at all.
408        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        // Find the polygon to connect to.
416        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        // findNearestPoly may return too optimistic results, check again.
423        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
437/// `connectIntLinks`.
438fn 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        // Build the edge links backwards so that the links will be in the linked list from the
450        // lowest index to the highest.
451        for j in (0..nv).rev() {
452            let nei = neis.get(j).copied().unwrap_or(0);
453            // Skip hard and non-internal edges.
454            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
470/// `baseOffMeshLinks` (with `connectExtOffMeshLinks(tile, tile, -1)` run by the caller).
471///
472/// DAYZ: every connection gets `reserved = 1.0` before the search, and the back-link from the
473/// landing polygon is only allocated when the forward link was.
474fn base_off_mesh_links(t: &mut MeshTile, base: u64) {
475    // Base the off-mesh connection start points.
476    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        // Find the polygon to connect to: the first vertex.
484        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        // findNearestPoly may return too optimistic results, check again.
491        if sqr(nearest[0] - p[0]) + sqr(nearest[2] - p[2]) > sqr(rad) {
492            continue;
493        }
494        // Make sure the location is on the current mesh.
495        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        // Link the off-mesh connection to the target polygon.
506        if push_link(t, con_poly, reference, 0, 0xff, Some((0, 0))) {
507            // The start end point is always connected back to the off-mesh connection.
508            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    /// Creates an empty mesh (`dtNavMesh::init`): every slot has salt 1 and slot 0 is allocated
523    /// first.
524    ///
525    /// # Errors
526    ///
527    /// Returns an error when `max_tiles` is not positive or exceeds what the 28-bit tile field of a
528    /// polygon reference can address.
529    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    /// Builds a mesh from a set file: the tiles are added in file order, each into the slot and
545    /// with the salt of its stored reference. Stored links are ignored and recomputed (spec 2.3).
546    ///
547    /// # Errors
548    ///
549    /// Returns an error for invalid parameters or a tile that [`NavMesh::set_tile`]'s checks
550    /// reject: wrong magic or version, an inconsistent header, indices out of range, an occupied
551    /// grid cell, or a slot that is out of range or already used.
552    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    /// The mesh parameters.
562    #[must_use]
563    pub fn params(&self) -> &NavMeshParams {
564        &self.params
565    }
566
567    /// Adds a tile, replacing the one in the same grid cell (spec 8.2). A replacement keeps its
568    /// slot and gets the salt plus one. Returns the reference of the tile.
569    ///
570    /// # Errors
571    ///
572    /// Returns an error, leaving the mesh unchanged, when the tile is rejected (see
573    /// [`NavMesh::from_set`]) or no slot is free.
574    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    /// Removes the tile in grid cell `(x, y)` and unlinks its neighbours. Returns whether there
588    /// was one.
589    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    /// The tile in grid cell `(x, y)`.
600    #[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    /// The tiles with their references, in slot order.
607    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    /// The mesh as a set file for a `tiles_x` by `tiles_z` grid: the tiles in slot order with
616    /// their references and record padding (kept from `from_set`, 0 for tiles added with `set_tile`).
617    #[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    /// Like [`NavMesh::to_set`], but consumes the mesh and moves the tiles instead of cloning them,
639    /// so the final save of a large run does not double the peak memory.
640    #[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    /// The tiles of the grid cells `min..=max` (`x`, `y`), with their reference bases, row by row
663    /// (`y` outer, `x` inner) as `dtNavMeshQuery::queryPolygons` visits them.
664    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        // Each side is at most 2^32, so the product can exceed `i64`: saturate (a huge box scans the grid).
673        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                // A box far larger than the grid: scan the grid instead of the cells.
683                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    /// The polygons of every tile, tile by tile in slot order, for the island filter.
699    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    /// The tile a polygon reference points into, with its slot, if the salt and the polygon index
706    /// are valid (`getTileAndPolyByRef`).
707    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    /// Checks everything the linker relies on, so that it never sees an index out of range.
732    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    /// `dtNavMesh::addTile` with `lastRef` (0: none); returns the tile reference.
799    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        // Build the link free list.
815        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    /// Takes the head of the free list, else the next never-used slot.
841    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    /// Takes the slot of `last_ref` out of the free list and restores its salt.
857    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    /// The linking half of `addTile`.
876    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 the off-mesh connections to their starting polygons and connect the
881            // connections inside the tile.
882            base_off_mesh_links(t, base);
883        }
884        self.connect_ext_off_mesh_links(slot, slot, -1);
885
886        // Create connections with the neighbour tiles.
887        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    /// `getNeighbourTilesAt`.
901    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    /// `connectExtLinks(tile, target, side)`.
918    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    /// `connectExtOffMeshLinks(tile, target, side)`; `tile == target` for `side == -1`.
932    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            // Link the off-mesh connection to the target polygon.
948            if let Some(tg) = self.tile_mut(target) {
949                // Make sure the location is on the current mesh.
950                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            // Link the target polygon to the off-mesh connection.
956            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    /// DAYZ (spec 8.3): snaps the y of every obstacle of the extra section to the nearest
978    /// polygon of its tile, within a vertical half extent of 30. Not idempotent for obstacles
979    /// that are not over a polygon.
980    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    /// `removeTile`: unlinks the neighbours, bumps the salt and puts the slot at the head of the
1007    /// free list.
1008    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    /// `unconnectLinks`: removes the links of `tile` that point into the tile in slot `target`.
1026    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            // A tile owns its links, so the chain ends; the bound only guards a corrupted pool.
1035            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                    // Remove the link.
1044                    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                    // Advance.
1058                    pj = j;
1059                    j = nj;
1060                }
1061            }
1062        }
1063    }
1064}