use std::io;
use crate::navmesh::detour::tile::{
BvNode, Link, MeshHeader, NAVMESH_MAGIC, NAVMESH_VERSION, OffMeshConnection, Poly, TileData,
TileExtra,
};
use crate::navmesh::num::{ceil_i32, floor_i32};
use crate::navmesh::qsort::msvc_qsort;
const MESH_NULL_IDX: u16 = 0xffff;
const EXT_LINK: u16 = 0x8000;
const VERTS_PER_POLYGON: usize = 6;
#[derive(Debug, Clone, Default, PartialEq)]
#[non_exhaustive]
pub struct OffMeshInput {
pub start: [f32; 3],
pub end: [f32; 3],
pub center_height: f32,
pub radius: f32,
pub flags: u16,
pub area: u8,
pub bidirectional: bool,
pub user_id: u32,
}
#[derive(Debug, Clone)]
#[non_exhaustive]
pub struct CreateParams<'a> {
pub verts: &'a [[u16; 3]],
pub polys: &'a [u16],
pub flags: &'a [u16],
pub areas: &'a [u8],
pub nvp: usize,
pub off_mesh: &'a [OffMeshInput],
pub extra: &'a TileExtra,
pub tile_x: i32,
pub tile_y: i32,
pub bmin: [f32; 3],
pub bmax: [f32; 3],
pub walkable_height: f32,
pub walkable_radius: f32,
pub walkable_climb: f32,
pub cs: f32,
pub ch: f32,
}
impl<'a> CreateParams<'a> {
#[must_use]
pub fn new(
verts: &'a [[u16; 3]],
polys: &'a [u16],
flags: &'a [u16],
areas: &'a [u8],
nvp: usize,
off_mesh: &'a [OffMeshInput],
extra: &'a TileExtra,
) -> Self {
Self {
verts,
polys,
flags,
areas,
nvp,
off_mesh,
extra,
tile_x: 0,
tile_y: 0,
bmin: [0.0; 3],
bmax: [0.0; 3],
walkable_height: 0.0,
walkable_radius: 0.0,
walkable_climb: 0.0,
cs: 0.25,
ch: 0.1,
}
}
}
pub(crate) fn classify_off_mesh_point(pt: &[f32; 3], bmin: &[f32; 3], bmax: &[f32; 3]) -> u8 {
const XP: u8 = 1;
const ZP: u8 = 2;
const XM: u8 = 4;
const ZM: u8 = 8;
let mut outcode = 0_u8;
if pt[0] >= bmax[0] {
outcode |= XP;
}
if pt[2] >= bmax[2] {
outcode |= ZP;
}
if pt[0] < bmin[0] {
outcode |= XM;
}
if pt[2] < bmin[2] {
outcode |= ZM;
}
match outcode {
XP => 0,
v if v == XP | ZP => 1,
ZP => 2,
v if v == XM | ZP => 3,
XM => 4,
v if v == XM | ZM => 5,
ZM => 6,
v if v == XP | ZM => 7,
_ => 0xff,
}
}
#[derive(Clone, Copy)]
struct BvItem {
bmin: [u16; 3],
bmax: [u16; 3],
i: i32,
}
fn clamp_u16(v: i32) -> u16 {
u16::try_from(v.clamp(0, i32::from(u16::MAX))).unwrap_or(u16::MAX)
}
fn longest_axis(x: u16, y: u16, z: u16) -> usize {
let mut axis = 0;
let mut max_val = x;
if y > max_val {
axis = 1;
max_val = y;
}
if z > max_val {
axis = 2;
}
axis
}
fn grow(bmin: &mut [u16; 3], bmax: &mut [u16; 3], vmin: [u16; 3], vmax: [u16; 3]) {
for (lo, v) in bmin.iter_mut().zip(vmin) {
if v < *lo {
*lo = v;
}
}
for (hi, v) in bmax.iter_mut().zip(vmax) {
if v > *hi {
*hi = v;
}
}
}
fn calc_extends(items: &[BvItem]) -> ([u16; 3], [u16; 3]) {
let Some(first) = items.first() else {
return ([0; 3], [0; 3]);
};
let mut bmin = first.bmin;
let mut bmax = first.bmax;
for it in items.iter().skip(1) {
grow(&mut bmin, &mut bmax, it.bmin, it.bmax);
}
(bmin, bmax)
}
fn subdivide(items: &mut [BvItem], nodes: &mut Vec<BvNode>) {
let icur = nodes.len();
nodes.push(BvNode::default());
let inum = items.len();
if let [only] = items {
if let Some(node) = nodes.get_mut(icur) {
node.bmin = only.bmin;
node.bmax = only.bmax;
node.i = only.i;
}
return;
}
let (bmin, bmax) = calc_extends(items);
let axis = longest_axis(
bmax[0].saturating_sub(bmin[0]),
bmax[1].saturating_sub(bmin[1]),
bmax[2].saturating_sub(bmin[2]),
);
msvc_qsort(items, |a, b| a.bmin.get(axis).cmp(&b.bmin.get(axis)));
let isplit = inum / 2;
let (left, right) = items.split_at_mut(isplit);
subdivide(left, nodes);
subdivide(right, nodes);
let escape = nodes.len().saturating_sub(icur);
let escape = i32::try_from(escape).unwrap_or(i32::MAX);
if let Some(node) = nodes.get_mut(icur) {
node.bmin = bmin;
node.bmax = bmax;
node.i = -escape;
}
}
fn create_bv_tree(
params: &CreateParams<'_>,
nvp: usize,
poly_count: usize,
) -> io::Result<Vec<BvNode>> {
let mut items = Vec::with_capacity(poly_count);
for (i, p) in params.polys.chunks_exact(nvp * 2).enumerate() {
let (idx, _) = p.split_at(nvp);
let vertex = |k: u16| -> io::Result<[u16; 3]> {
params
.verts
.get(usize::from(k))
.copied()
.ok_or_else(|| error!("polygon {i} uses vertex {k} outside the vertex list"))
};
let first = vertex(idx.first().copied().unwrap_or(MESH_NULL_IDX))?;
let mut bmin = first;
let mut bmax = first;
for &k in idx.iter().skip(1) {
if k == MESH_NULL_IDX {
break;
}
let v = vertex(k)?;
grow(&mut bmin, &mut bmax, v, v);
}
bmin[1] = clamp_u16(floor_i32(f32::from(bmin[1]) * params.ch / params.cs));
bmax[1] = clamp_u16(ceil_i32(f32::from(bmax[1]) * params.ch / params.cs));
items.push(BvItem {
bmin,
bmax,
i: i32::try_from(i).map_err(|_| error!("too many polygons"))?,
});
}
let mut nodes = Vec::with_capacity(poly_count.saturating_mul(2));
subdivide(&mut items, &mut nodes);
Ok(nodes)
}
fn count_i32(n: usize, what: &str) -> io::Result<i32> {
i32::try_from(n).map_err(|_| error!("too many {what}"))
}
struct OffMeshClasses {
classes: Vec<(u8, u8)>,
stored: usize,
link_count: usize,
}
fn classify_off_mesh(params: &CreateParams<'_>) -> OffMeshClasses {
let mut out = OffMeshClasses {
classes: Vec::with_capacity(params.off_mesh.len()),
stored: 0,
link_count: 0,
};
if params.off_mesh.is_empty() {
return out;
}
let mut hmin = f32::MAX;
let mut hmax = -f32::MAX;
for iv in params.verts {
let h = params.bmin[1] + f32::from(iv[1]) * params.ch;
hmin = if hmin < h { hmin } else { h };
hmax = if hmax > h { hmax } else { h };
}
hmin -= params.walkable_climb;
hmax += params.walkable_climb;
let mut bmin = params.bmin;
let mut bmax = params.bmax;
bmin[1] = hmin;
bmax[1] = hmax;
for c in params.off_mesh {
let mut start = classify_off_mesh_point(&c.start, &bmin, &bmax);
let end = classify_off_mesh_point(&c.end, &bmin, &bmax);
if start == 0xff && (c.start[1] < bmin[1] || c.start[1] > bmax[1]) {
start = 0;
}
if start == 0xff {
out.link_count += 1;
out.stored += 1;
}
if end == 0xff {
out.link_count += 1;
}
out.classes.push((start, end));
}
out
}
pub(crate) fn stored_off_mesh(params: &CreateParams<'_>) -> Vec<bool> {
classify_off_mesh(params)
.classes
.iter()
.map(|&(start, _)| start == 0xff)
.collect()
}
fn validate(params: &CreateParams<'_>) -> io::Result<usize> {
let nvp = params.nvp;
if nvp > VERTS_PER_POLYGON {
return Err(error!(
"{nvp} vertices per polygon (at most {VERTS_PER_POLYGON})"
));
}
if nvp == 0 {
return Err(error!("no vertices per polygon"));
}
if params.verts.len() >= usize::from(MESH_NULL_IDX) {
return Err(error!("too many vertices ({})", params.verts.len()));
}
if params.verts.is_empty() {
return Err(error!("no vertices"));
}
if params.polys.is_empty() {
return Err(error!("no polygons"));
}
let stride = nvp * 2;
if !params.polys.len().is_multiple_of(stride) {
return Err(error!(
"polygon array of {} values is not a multiple of {stride}",
params.polys.len()
));
}
let poly_count = params.polys.len() / stride;
if params.flags.len() != poly_count || params.areas.len() != poly_count {
return Err(error!(
"{poly_count} polygons but {} flags and {} areas",
params.flags.len(),
params.areas.len()
));
}
Ok(poly_count)
}
fn count_edges(params: &CreateParams<'_>) -> (usize, usize) {
let nvp = params.nvp;
let mut edge_count = 0_usize;
let mut portal_count = 0_usize;
for p in params.polys.chunks_exact(nvp * 2) {
let (idx, nei) = p.split_at(nvp);
for (&k, &n) in idx.iter().zip(nei) {
if k == MESH_NULL_IDX {
break;
}
edge_count += 1;
if n & EXT_LINK != 0 && n & 0xf != 0xf {
portal_count += 1;
}
}
}
(edge_count, portal_count)
}
fn neighbour_code(n: u16) -> u16 {
if n & EXT_LINK == 0 {
return n + 1;
}
match n & 0xf {
0 => EXT_LINK | 4, 1 => EXT_LINK | 2, 2 => EXT_LINK, 3 => EXT_LINK | 6, _ => 0, }
}
fn ground_polys(params: &CreateParams<'_>) -> io::Result<Vec<Poly>> {
let nvp = params.nvp;
let mut polys = Vec::new();
for (i, p) in params.polys.chunks_exact(nvp * 2).enumerate() {
let (idx, nei) = p.split_at(nvp);
let mut poly = Poly {
flags: params.flags.get(i).copied().unwrap_or(0),
..Poly::default()
};
poly.set_area(params.areas.get(i).copied().unwrap_or(0));
poly.set_type(0);
for (j, (&k, &n)) in idx.iter().zip(nei).enumerate() {
if k == MESH_NULL_IDX {
break;
}
if usize::from(k) >= params.verts.len() {
return Err(error!(
"polygon {i} uses vertex {k} outside the vertex list"
));
}
if let Some(slot) = poly.verts.get_mut(j) {
*slot = k;
}
if let Some(slot) = poly.neis.get_mut(j) {
*slot = neighbour_code(n);
}
poly.vert_count += 1;
}
if poly.vert_count == 0 {
return Err(error!("polygon {i} has no vertices"));
}
polys.push(poly);
}
Ok(polys)
}
fn off_mesh_parts(
params: &CreateParams<'_>,
classes: &[(u8, u8)],
verts: &mut Vec<[f32; 3]>,
polys: &mut Vec<Poly>,
) -> io::Result<Vec<OffMeshConnection>> {
let poly_base = polys.len();
let mut cons = Vec::new();
for (c, &(start, end)) in params.off_mesh.iter().zip(classes) {
if start != 0xff {
continue;
}
let n = cons.len();
let first = u16::try_from(verts.len())
.map_err(|_| error!("too many vertices with the off-mesh connections"))?;
let second = first
.checked_add(1)
.ok_or_else(|| error!("too many vertices with the off-mesh connections"))?;
let poly = u16::try_from(poly_base + n)
.map_err(|_| error!("too many polygons with the off-mesh connections"))?;
verts.push(c.start);
verts.push(c.end);
let mut off = Poly {
vert_count: 2,
flags: c.flags,
..Poly::default()
};
off.verts = [first, second, 0, 0, 0, 0];
off.set_area(c.area);
off.set_type(1);
polys.push(off);
cons.push(OffMeshConnection {
pos: [
c.start[0], c.start[1], c.start[2], c.end[0], c.end[1], c.end[2],
],
center_height: c.center_height,
reserved: 0.0,
rad: c.radius,
poly,
flags: u8::from(c.bidirectional),
side: end,
user_id: c.user_id,
});
}
Ok(cons)
}
pub fn create_tile_data(params: &CreateParams<'_>) -> io::Result<TileData> {
let poly_count = validate(params)?;
let off = classify_off_mesh(params);
let (edge_count, portal_count) = count_edges(params);
let max_link_count = edge_count + portal_count * 2 + off.link_count * 2;
let mut verts: Vec<[f32; 3]> = params
.verts
.iter()
.map(|iv| {
[
params.bmin[0] + f32::from(iv[0]) * params.cs,
params.bmin[1] + f32::from(iv[1]) * params.ch,
params.bmin[2] + f32::from(iv[2]) * params.cs,
]
})
.collect();
let mut polys = ground_polys(params)?;
let off_mesh = off_mesh_parts(params, &off.classes, &mut verts, &mut polys)?;
let mut bv_tree = create_bv_tree(params, params.nvp, poly_count)?;
bv_tree.resize(poly_count * 2, BvNode::default());
let header = MeshHeader {
magic: NAVMESH_MAGIC,
version: NAVMESH_VERSION,
x: params.tile_x,
y: params.tile_y,
layer: 0,
user_id: 0,
poly_count: count_i32(polys.len(), "polygons")?,
vert_count: count_i32(verts.len(), "vertices")?,
max_link_count: count_i32(max_link_count, "links")?,
detail_mesh_count: 0,
detail_vert_count: 0,
detail_tri_count: 0,
bv_node_count: count_i32(poly_count * 2, "polygons")?,
off_mesh_con_count: count_i32(off_mesh.len(), "off-mesh connections")?,
off_mesh_base: count_i32(poly_count, "polygons")?,
walkable_height: params.walkable_height,
walkable_radius: params.walkable_radius,
walkable_climb: params.walkable_climb,
bmin: params.bmin,
bmax: params.bmax,
bv_quant_factor: 1.0 / params.cs,
extra_data_size: count_i32(params.extra.byte_size(), "extra section bytes")?,
};
Ok(TileData {
header,
verts,
polys,
links: vec![Link::default(); max_link_count],
detail_meshes: Vec::new(),
detail_verts: Vec::new(),
detail_tris: Vec::new(),
bv_tree,
off_mesh,
extra: Some(params.extra.clone()),
})
}