"""NavigationMesh3D: triangle-based navmesh with A* pathfinding."""
import heapq
import logging
import math
import random
import numpy as np
from ..math.types import Vec3
from ._helpers import (
_closest_point_on_triangle_np,
_interpolate_height,
_point_in_polygon_xz,
_point_in_triangle_xz,
_vec3_dist,
)
log = logging.getLogger(__name__)
def _segments_cross_xz(
ax: float,
az: float,
bx: float,
bz: float,
cx: float,
cz: float,
dx: float,
dz: float,
) -> bool:
"""Test whether segments a-b and c-d properly cross in the XZ plane.
"Properly" means each segment has one endpoint strictly to either side of
the other's line. Segments that merely touch at an endpoint, or that lie
along the same line, are not crossings: they enclose no area, so they cannot
on their own make two polygons overlap.
"""
def side(px: float, pz: float, qx: float, qz: float, rx: float, rz: float) -> float:
return (qx - px) * (rz - pz) - (qz - pz) * (rx - px)
d1 = side(cx, cz, dx, dz, ax, az)
d2 = side(cx, cz, dx, dz, bx, bz)
if not ((d1 > 0.0 > d2) or (d2 > 0.0 > d1)):
return False
d3 = side(ax, az, bx, bz, cx, cz)
d4 = side(ax, az, bx, bz, dx, dz)
return (d3 > 0.0 > d4) or (d4 > 0.0 > d3)
def _dist_xyz(a: tuple[float, float, float], b: tuple[float, float, float]) -> float:
"""Euclidean distance between two positions held as plain float triples."""
dx, dy, dz = a[0] - b[0], a[1] - b[1], a[2] - b[2]
return math.sqrt(dx * dx + dy * dy + dz * dz)
def _closest_point_on_segment_xz(
px: float,
pz: float,
ax: float,
az: float,
bx: float,
bz: float,
) -> tuple[float, float]:
"""Closest point to (px, pz) on the segment a-b, in the XZ plane."""
dx, dz = bx - ax, bz - az
length_sq = dx * dx + dz * dz
if length_sq < 1e-18:
return ax, az
t = ((px - ax) * dx + (pz - az) * dz) / length_sq
t = 0.0 if t < 0.0 else (1.0 if t > 1.0 else t)
return ax + t * dx, az + t * dz
[docs]
class NavigationMesh3D:
"""3D navigation mesh with triangle-based A* pathfinding.
Stores walkable geometry as triangles and builds an adjacency graph for
pathfinding. Supports manual polygon insertion and automated bake from
level geometry.
"""
def __init__(self):
self._vertices: list[list[float]] = [] # flat list of [x, y, z]
self._triangles: list[list[int]] = [] # each is [i0, i1, i2]
self._adjacency: dict[int, set[int]] = {} # triangle index -> neighbor triangle indices
self._obstacles: list[list[list[float]]] = [] # each obstacle is a list of [x, z] boundary points
self._obstacle_bounds: list[tuple[float, float, float, float]] = [] # (x_min, z_min, x_max, z_max)
self._blocked: set[int] = set() # triangle indices blocked by obstacles
self._dirty = True
# Caches rebuilt with the adjacency graph, all invalidated by _dirty.
self._tri_v = np.zeros((0, 3, 3), dtype=np.float64) # (M, 3, 3) triangle corner positions
self._centroids: list[tuple[float, float, float]] = []
self._grid: list[list[int]] = [] # uniform XZ buckets, row-major, of triangle indices
self._grid_nx = 0
self._grid_nz = 0
self._grid_x0 = 0.0
self._grid_z0 = 0.0
self._grid_cw = 1.0
self._grid_ch = 1.0
# -- Public numpy views --
[docs]
@property
def vertices(self) -> np.ndarray:
"""(N, 3) float32 array of mesh vertices."""
if not self._vertices:
return np.zeros((0, 3), dtype=np.float32)
return np.array(self._vertices, dtype=np.float32)
[docs]
@property
def triangles(self) -> np.ndarray:
"""(M, 3) int32 array of triangle vertex indices."""
if not self._triangles:
return np.zeros((0, 3), dtype=np.int32)
return np.array(self._triangles, dtype=np.int32)
[docs]
@property
def triangle_count(self) -> int:
return len(self._triangles)
# -- Polygon insertion --
[docs]
def add_polygon(self, vertices: list[Vec3], cell_size: float = 0.0) -> None:
"""Add a walkable polygon.
Without ``cell_size`` the polygon is fan-triangulated from the first
vertex (fast, 2 triangles for a quad). When ``cell_size`` is positive
the polygon's axis-aligned bounding box is subdivided into a regular
grid of right-triangles, keeping only cells whose centres fall inside
the polygon. The finer mesh is required for obstacle carving to work
at useful resolution.
Args:
vertices: 3+ coplanar points defining the polygon boundary.
cell_size: When > 0, grid cell size for subdivision. Typical
values: 0.5 -- 2.0 depending on obstacle density.
"""
if len(vertices) < 3:
raise ValueError("Polygon requires at least 3 vertices")
if cell_size > 0:
self._add_polygon_subdivided(vertices, cell_size)
else:
base_idx = len(self._vertices)
for v in vertices:
self._vertices.append([v.x, v.y, v.z])
for i in range(1, len(vertices) - 1):
self._triangles.append([base_idx, base_idx + i, base_idx + i + 1])
self._dirty = True
def _add_polygon_subdivided(self, vertices: list[Vec3], cell_size: float) -> None:
"""Grid-subdivide a polygon into fine triangles for obstacle carving."""
poly_xz = [[v.x, v.z] for v in vertices]
# Use average Y for the flat surface
avg_y = sum(v.y for v in vertices) / len(vertices)
# Bounding box
xs = [v.x for v in vertices]
zs = [v.z for v in vertices]
x_min, x_max = min(xs), max(xs)
z_min, z_max = min(zs), max(zs)
nx = max(1, int(math.ceil((x_max - x_min) / cell_size)))
nz = max(1, int(math.ceil((z_max - z_min) / cell_size)))
dx = (x_max - x_min) / nx
dz = (z_max - z_min) / nz
# Create grid vertices
base = len(self._vertices)
grid: dict[tuple[int, int], int] = {} # (ix, iz) -> vertex index
for iz in range(nz + 1):
for ix in range(nx + 1):
gx = x_min + ix * dx
gz = z_min + iz * dz
idx = base + len(grid)
grid[(ix, iz)] = idx
self._vertices.append([gx, avg_y, gz])
# Create triangles for cells whose centre is inside the polygon
for iz in range(nz):
for ix in range(nx):
cx = x_min + (ix + 0.5) * dx
cz = z_min + (iz + 0.5) * dz
if not _point_in_polygon_xz(cx, cz, poly_xz):
continue
i0 = grid[(ix, iz)]
i1 = grid[(ix + 1, iz)]
i2 = grid[(ix + 1, iz + 1)]
i3 = grid[(ix, iz + 1)]
self._triangles.append([i0, i1, i2])
self._triangles.append([i0, i2, i3])
[docs]
def add_triangle(self, v0: Vec3, v1: Vec3, v2: Vec3) -> None:
"""Add a single walkable triangle."""
base_idx = len(self._vertices)
self._vertices.append([v0.x, v0.y, v0.z])
self._vertices.append([v1.x, v1.y, v1.z])
self._vertices.append([v2.x, v2.y, v2.z])
self._triangles.append([base_idx, base_idx + 1, base_idx + 2])
self._dirty = True
[docs]
def add_obstacle(self, vertices: list[Vec3]) -> None:
"""Subtract an obstacle region from the walkable area.
The polygon is projected onto the XZ plane. Every navmesh triangle whose
area overlaps it, by any amount, is marked as blocked and excluded from
pathfinding and from the spatial queries.
Carving works at the resolution of the mesh, since a triangle is either
blocked or it is not. The hole is therefore never smaller than the
obstacle, and never larger than the obstacle grown by one triangle of
the mesh it is cut from. Pass ``cell_size`` to :meth:`add_polygon` to
make those triangles small enough that the difference does not matter.
Args:
vertices: 3+ points defining the obstacle boundary (Y is ignored).
"""
if len(vertices) < 3:
raise ValueError("Obstacle polygon requires at least 3 vertices")
self._obstacles.append([[v.x, v.z] for v in vertices])
xs = [v.x for v in vertices]
zs = [v.z for v in vertices]
self._obstacle_bounds.append((min(xs), min(zs), max(xs), max(zs)))
self._dirty = True
[docs]
def clear(self) -> None:
"""Remove all polygons, obstacles, and cached data."""
self._vertices.clear()
self._triangles.clear()
self._adjacency.clear()
self._obstacles.clear()
self._obstacle_bounds.clear()
self._blocked.clear()
self._dirty = True
# -- Geometry baking --
[docs]
def bake_from_geometry(
self,
mesh_vertices: np.ndarray,
mesh_indices: np.ndarray,
agent_radius: float = 0.5,
agent_height: float = 2.0,
max_slope: float = 45.0,
cell_size: float = 0.3,
cell_height: float = 0.2,
) -> None:
"""Generate navmesh from level geometry using simplified Recast-style algorithm.
Steps:
1. Voxelize geometry into a height field
2. Mark walkable voxels (slope < max_slope, clearance > agent_height)
3. Build regions from connected walkable areas
4. Extract contours and triangulate
Args:
mesh_vertices: (N, 3) float32 array of source mesh vertices.
mesh_indices: (M, 3) int32 array of triangle indices.
agent_radius: Agent capsule radius for erosion.
agent_height: Minimum clearance height.
max_slope: Maximum walkable slope in degrees.
cell_size: Horizontal voxel size.
cell_height: Vertical voxel size.
"""
mesh_vertices = np.asarray(mesh_vertices, dtype=np.float32)
mesh_indices = np.asarray(mesh_indices, dtype=np.int32)
if mesh_vertices.ndim != 2 or mesh_vertices.shape[1] != 3:
raise ValueError("mesh_vertices must be (N, 3)")
if mesh_indices.ndim != 2 or mesh_indices.shape[1] != 3:
raise ValueError("mesh_indices must be (M, 3)")
# Step 1: Compute bounding box
bmin = mesh_vertices.min(axis=0)
bmax = mesh_vertices.max(axis=0)
grid_w = max(1, int(math.ceil((bmax[0] - bmin[0]) / cell_size)))
grid_d = max(1, int(math.ceil((bmax[2] - bmin[2]) / cell_size)))
# Step 2: Rasterize triangles into height field, filter by slope
cos_max = math.cos(math.radians(max_slope))
up = np.array([0.0, 1.0, 0.0], dtype=np.float32)
# height_field[x, z] = list of (y_min, y_max) spans
height_field: dict[tuple[int, int], list[float]] = {}
for tri in mesh_indices:
v0, v1, v2 = mesh_vertices[tri[0]], mesh_vertices[tri[1]], mesh_vertices[tri[2]]
# Compute triangle normal
e1 = v1 - v0
e2 = v2 - v0
normal = np.cross(e1, e2)
norm_len = np.linalg.norm(normal)
if norm_len < 1e-10:
continue
normal /= norm_len
# Check slope
if abs(np.dot(normal, up)) < cos_max:
continue
# Rasterize: project triangle onto xz grid
tri_verts = np.array([v0, v1, v2])
tmin = tri_verts.min(axis=0)
tmax = tri_verts.max(axis=0)
ix0 = max(0, int((tmin[0] - bmin[0]) / cell_size))
ix1 = min(grid_w - 1, int((tmax[0] - bmin[0]) / cell_size))
iz0 = max(0, int((tmin[2] - bmin[2]) / cell_size))
iz1 = min(grid_d - 1, int((tmax[2] - bmin[2]) / cell_size))
for ix in range(ix0, ix1 + 1):
for iz in range(iz0, iz1 + 1):
cx = bmin[0] + (ix + 0.5) * cell_size
cz = bmin[2] + (iz + 0.5) * cell_size
# Point-in-triangle test (projected to xz)
y = _interpolate_height(v0, v1, v2, cx, cz)
if y is not None:
key = (ix, iz)
if key not in height_field:
height_field[key] = []
height_field[key].append(y)
# Step 3: Build walkable cells: keep highest walkable surface per cell
walkable: dict[tuple[int, int], float] = {}
for key, heights in height_field.items():
walkable[key] = max(heights)
# Step 4: Erode by agent radius
erode_cells = max(1, int(math.ceil(agent_radius / cell_size)))
eroded: dict[tuple[int, int], float] = {}
for (ix, iz), y in walkable.items():
walkable_neighbors = True
for dx in range(-erode_cells, erode_cells + 1):
for dz in range(-erode_cells, erode_cells + 1):
if (ix + dx, iz + dz) not in walkable:
walkable_neighbors = False
break
if not walkable_neighbors:
break
if walkable_neighbors:
eroded[(ix, iz)] = y
# Step 5: Build regions via flood fill
visited: set[tuple[int, int]] = set()
regions: list[list[tuple[int, int]]] = []
for cell in eroded:
if cell in visited:
continue
region: list[tuple[int, int]] = []
stack = [cell]
while stack:
c = stack.pop()
if c in visited or c not in eroded:
continue
# Check height continuity
if region and abs(eroded[c] - eroded[region[0]]) > agent_height:
continue
visited.add(c)
region.append(c)
ix, iz = c
for nx, nz in [(ix + 1, iz), (ix - 1, iz), (ix, iz + 1), (ix, iz - 1)]:
if (nx, nz) not in visited and (nx, nz) in eroded:
stack.append((nx, nz))
if len(region) >= 2:
regions.append(region)
# Step 6: Triangulate each region
self._vertices.clear()
self._triangles.clear()
for region in regions:
self._triangulate_region(region, eroded, bmin, cell_size)
self._dirty = True
def _triangulate_region(
self,
cells: list[tuple[int, int]],
heights: dict[tuple[int, int], float],
bmin: np.ndarray,
cell_size: float,
) -> None:
"""Triangulate a connected region of walkable cells using a grid-based approach."""
cell_set = set(cells)
# Map cell -> vertex index
cell_to_idx: dict[tuple[int, int], int] = {}
base = len(self._vertices)
for ix, iz in cells:
idx = base + len(cell_to_idx)
cell_to_idx[(ix, iz)] = idx
cx = bmin[0] + (ix + 0.5) * cell_size
cz = bmin[2] + (iz + 0.5) * cell_size
self._vertices.append([cx, heights[(ix, iz)], cz])
# Create triangles from adjacent cell quads
for ix, iz in cells:
# Right and down neighbors form a quad -> 2 triangles
r = (ix + 1, iz)
d = (ix, iz + 1)
rd = (ix + 1, iz + 1)
if r in cell_set and d in cell_set and rd in cell_set:
i0 = cell_to_idx[(ix, iz)]
i1 = cell_to_idx[r]
i2 = cell_to_idx[rd]
i3 = cell_to_idx[d]
self._triangles.append([i0, i1, i2])
self._triangles.append([i0, i2, i3])
# -- Adjacency graph --
def _build_adjacency(self) -> None:
"""Build triangle adjacency from shared edges and mark obstacle-blocked triangles."""
if not self._dirty:
return
self._adjacency.clear()
self._blocked.clear()
# Merge vertices at the same position (quantized to avoid float issues)
SNAP = 1e-4
pos_to_canonical: dict[tuple[int, int, int], int] = {}
vertex_map: dict[int, int] = {} # original index -> canonical index
for i, v in enumerate(self._vertices):
key = (round(v[0] / SNAP), round(v[1] / SNAP), round(v[2] / SNAP))
if key not in pos_to_canonical:
pos_to_canonical[key] = i
vertex_map[i] = pos_to_canonical[key]
# Map edge (sorted canonical vertex pair) -> list of triangle indices
edge_to_tris: dict[tuple[int, int], list[int]] = {}
for ti, tri in enumerate(self._triangles):
self._adjacency.setdefault(ti, set())
for j in range(3):
ca = vertex_map[tri[j]]
cb = vertex_map[tri[(j + 1) % 3]]
edge = (min(ca, cb), max(ca, cb))
if edge not in edge_to_tris:
edge_to_tris[edge] = []
edge_to_tris[edge].append(ti)
for tris in edge_to_tris.values():
for i, tri_a in enumerate(tris):
for j in range(i + 1, len(tris)):
self._adjacency[tri_a].add(tris[j])
self._adjacency[tris[j]].add(tri_a)
# Mark triangles blocked by obstacles
if self._obstacles:
for ti in range(len(self._triangles)):
if self._is_triangle_blocked(ti):
self._blocked.add(ti)
self._build_index()
self._dirty = False
def _build_index(self) -> None:
"""Cache triangle corners and centroids, and bucket the triangles into a uniform XZ grid.
Every spatial query on this class asks "which triangles are near this
point in XZ", so the grid is what keeps a query from walking the whole
mesh: a triangle is filed under every cell its XZ bounding box touches,
and a query reads the cells outwards from the one holding the point
until no unread cell can hold anything nearer than the best result so
far.
Cells are sized to hold a couple of triangles each, but never made so
small that a typical triangle spans many of them, since a triangle
filed under a hundred cells costs more to index than it saves.
"""
count = len(self._triangles)
if not count:
self._tri_v = np.zeros((0, 3, 3), dtype=np.float64)
self._centroids = []
self._grid = []
self._grid_nx = self._grid_nz = 0
return
verts = np.asarray(self._vertices, dtype=np.float64)
self._tri_v = verts[np.asarray(self._triangles, dtype=np.intp)]
self._centroids = [(float(x), float(y), float(z)) for x, y, z in self._tri_v.mean(axis=1).tolist()]
lo = self._tri_v.min(axis=1)
hi = self._tri_v.max(axis=1)
x0, z0 = float(lo[:, 0].min()), float(lo[:, 2].min())
span_x = max(float(hi[:, 0].max()) - x0, 1e-9)
span_z = max(float(hi[:, 2].max()) - z0, 1e-9)
target = max(1, int(math.sqrt(count / 2.0)))
mean_w = float((hi[:, 0] - lo[:, 0]).mean())
mean_d = float((hi[:, 2] - lo[:, 2]).mean())
nx = max(1, min(target, int(span_x / mean_w))) if mean_w > 0.0 else target
nz = max(1, min(target, int(span_z / mean_d))) if mean_d > 0.0 else target
nx, nz = max(1, nx), max(1, nz)
cw, ch = span_x / nx, span_z / nz
ix0 = np.clip(((lo[:, 0] - x0) / cw).astype(np.intp), 0, nx - 1)
ix1 = np.clip(((hi[:, 0] - x0) / cw).astype(np.intp), 0, nx - 1)
iz0 = np.clip(((lo[:, 2] - z0) / ch).astype(np.intp), 0, nz - 1)
iz1 = np.clip(((hi[:, 2] - z0) / ch).astype(np.intp), 0, nz - 1)
grid: list[list[int]] = [[] for _ in range(nx * nz)]
for ti in range(count):
for iz in range(int(iz0[ti]), int(iz1[ti]) + 1):
row = iz * nx
for ix in range(int(ix0[ti]), int(ix1[ti]) + 1):
grid[row + ix].append(ti)
self._grid = grid
self._grid_nx, self._grid_nz = nx, nz
self._grid_x0, self._grid_z0 = x0, z0
self._grid_cw, self._grid_ch = cw, ch
def _cell_of(self, x: float, z: float) -> tuple[int, int]:
"""Return the grid cell holding (x, z), clamped to the grid."""
ix = int((x - self._grid_x0) / self._grid_cw)
iz = int((z - self._grid_z0) / self._grid_ch)
ix = 0 if ix < 0 else min(ix, self._grid_nx - 1)
iz = 0 if iz < 0 else min(iz, self._grid_nz - 1)
return ix, iz
def _is_triangle_blocked(self, tri_idx: int) -> bool:
"""Report whether a triangle overlaps any obstacle polygon in the XZ projection.
Two polygons in the plane overlap exactly when a vertex of one lies
inside the other or a pair of their edges cross, so all three cases are
tested. That is an exact overlap test rather than a sample of a few
points: an obstacle small enough to sit entirely within one triangle is
found, and a triangle is not blocked merely because an obstacle happens
to cover one of its corners. The obstacle may be concave.
"""
tri = self._triangles[tri_idx]
v0, v1, v2 = self._vertices[tri[0]], self._vertices[tri[1]], self._vertices[tri[2]]
ax, az = v0[0], v0[2]
bx, bz = v1[0], v1[2]
cx, cz = v2[0], v2[2]
tri_xz = ((ax, az), (bx, bz), (cx, cz))
tx_min, tx_max = min(ax, bx, cx), max(ax, bx, cx)
tz_min, tz_max = min(az, bz, cz), max(az, bz, cz)
for obstacle, (ox_min, oz_min, ox_max, oz_max) in zip(self._obstacles, self._obstacle_bounds, strict=True):
# Disjoint bounding boxes cannot overlap, and most pairs are disjoint.
if tx_max < ox_min or tx_min > ox_max or tz_max < oz_min or tz_min > oz_max:
continue
if any(_point_in_polygon_xz(px, pz, obstacle) for px, pz in tri_xz):
return True
if any(_point_in_triangle_xz(ox, oz, ax, az, bx, bz, cx, cz) for ox, oz in obstacle):
return True
# Neither shape contains a vertex of the other, yet their outlines
# may still cross: an obstacle band laid straight across a triangle.
for i in range(len(obstacle)):
ox0, oz0 = obstacle[i]
ox1, oz1 = obstacle[(i + 1) % len(obstacle)]
for j in range(3):
tx0, tz0 = tri_xz[j]
tx1, tz1 = tri_xz[(j + 1) % 3]
if _segments_cross_xz(tx0, tz0, tx1, tz1, ox0, oz0, ox1, oz1):
return True
return False
# -- Triangle utilities --
def _point_in_triangle_3d(self, point: Vec3, tri_idx: int) -> bool:
"""Test if a point (projected to xz) lies within a triangle."""
tri = self._triangles[tri_idx]
v0, v1, v2 = self._vertices[tri[0]], self._vertices[tri[1]], self._vertices[tri[2]]
return _point_in_triangle_xz(point.x, point.z, v0[0], v0[2], v1[0], v1[2], v2[0], v2[2])
def _closest_point_on_triangle(self, point: Vec3, tri_idx: int) -> Vec3:
"""Find the closest point on a triangle to the given point.
Reads the cached corner positions, so the caller must have built the
index (every public entry point does, through :meth:`_build_adjacency`).
"""
v = self._tri_v[tri_idx]
p = np.array([point.x, point.y, point.z], dtype=np.float64)
return Vec3(*_closest_point_on_triangle_np(p, v[0], v[1], v[2]))
def _project_onto_triangle_xz(self, point: Vec3, tri_idx: int) -> Vec3:
"""Move a point onto a triangle in the XZ plane, keeping its height.
Triangles are carved in the XZ projection and this move is made in it
too, so a point is only ever pulled sideways onto the walkable surface,
never up or down: an agent standing a metre above the floor still
starts its path a metre above the floor. Height decides *which*
triangle a point belongs to (see :meth:`_find_triangle`), never where
on it the point ends up. A point already over the triangle is returned
unchanged, however far above or below it the caller placed it; deciding
whether that height is acceptable is the caller's job, and
:meth:`find_path` does it with ``max_distance``.
"""
if self._point_in_triangle_3d(point, tri_idx):
return Vec3(point)
tri = self._triangles[tri_idx]
corners = [(self._vertices[i][0], self._vertices[i][2]) for i in tri]
best_x, best_z = corners[0]
best_dist_sq = float("inf")
for j in range(3):
ax, az = corners[j]
bx, bz = corners[(j + 1) % 3]
cx, cz = _closest_point_on_segment_xz(point.x, point.z, ax, az, bx, bz)
d = (cx - point.x) ** 2 + (cz - point.z) ** 2
if d < best_dist_sq:
best_dist_sq = d
best_x, best_z = cx, cz
return Vec3(best_x, point.y, best_z)
def _find_triangle(self, point: Vec3) -> int | None:
"""Find the non-blocked triangle a point stands on. Returns its index, or None.
Containment is decided in the XZ projection, so a stack of floors over
one footprint contains a point equally well at every storey. The
candidate whose surface is vertically nearest the point wins, so an
agent on an upper floor is matched to the floor it is standing on
rather than to whichever storey comes first in the triangle list.
"""
self._build_adjacency()
if not self._triangles:
return None
px, py, pz = point.x, point.y, point.z
ix, iz = self._cell_of(px, pz)
best: int | None = None
best_drop = float("inf")
for i in self._grid[iz * self._grid_nx + ix]:
if i in self._blocked:
continue
v = self._tri_v[i]
y = _interpolate_height(v[0], v[1], v[2], px, pz)
if y is None:
continue
drop = abs(y - py)
if drop < best_drop:
best_drop = drop
best = i
return best
def _find_triangle_within(self, point: Vec3, max_distance: float) -> int | None:
"""Find the triangle a point belongs to, no further away than max_distance.
Containment is decided in the XZ projection, so a point hovering a
kilometre over the footprint contains just as well as one standing on
it and would otherwise short-circuit the bound entirely. A containing
triangle too far below (or above) therefore falls through to the
nearest-surface search rather than being rejected outright: that search
ranks in three dimensions, so an endpoint out of reach of the storey it
stands over still reaches a neighbouring triangle beside it.
"""
tri_idx = self._find_triangle(point)
if tri_idx is not None:
surface = self._closest_point_on_triangle(point, tri_idx)
if (surface - point).length_squared() <= max_distance * max_distance:
return tri_idx
return self._find_closest_triangle(point, max_distance)
# -- Pathfinding --
[docs]
def find_path(self, start: Vec3, end: Vec3, max_distance: float = float("inf")) -> list[Vec3]:
"""A* pathfinding on the navmesh triangle graph.
Finds the shortest route from start to end by searching through
connected, unblocked triangles. The returned waypoints are triangle
centroids bracketed by the start and end pulled sideways onto the
walkable surface, so a route that skirts a carved obstacle does not
finish inside it. That is a move in the XZ plane only: an endpoint keeps
the height it was given, and an endpoint already over walkable ground
within ``max_distance`` is returned untouched.
Where storeys stack over one footprint, an endpoint is matched to the
one whose surface is vertically nearest it, so an agent on a gallery is
routed along the gallery rather than along the floor beneath it.
Args:
start: Start position in world space.
end: End position in world space.
max_distance: How far start or end may lie from the walkable
surface, measured in three dimensions, and still be accepted.
A point off the mesh, inside a carved obstacle, or hovering
over the mesh footprint further than this above or below it,
is moved to the nearest walkable triangle when one is within
this distance, and the query fails otherwise. Unbounded by
default.
Returns:
List of Vec3 waypoints, or empty list if unreachable.
"""
self._build_adjacency()
if not self._triangles:
return []
# Snap only the end that needs it: a point already standing on a
# walkable triangle, within the bound, keeps that triangle.
start_tri = self._find_triangle_within(start, max_distance)
end_tri = self._find_triangle_within(end, max_distance)
if start_tri is None or end_tri is None:
return []
first = self._project_onto_triangle_xz(start, start_tri)
last = self._project_onto_triangle_xz(end, end_tri)
if start_tri == end_tri:
return [first, last]
# A* search over triangle graph
open_set: list[tuple[float, int, int | None]] = [] # (f_cost, tri_idx, parent)
heapq.heappush(open_set, (0.0, start_tri, -1))
came_from: dict[int, int] = {}
g_score: dict[int, float] = {start_tri: 0.0}
centroids = self._centroids
end_centroid = centroids[end_tri]
while open_set:
f, current, _ = heapq.heappop(open_set)
if current == end_tri:
# Reconstruct path
path_tris = [current]
while current in came_from:
current = came_from[current]
path_tris.append(current)
path_tris.reverse()
# Build waypoint list: start -> centroids -> end
waypoints = [first]
for ti in path_tris[1:-1]:
waypoints.append(Vec3(*centroids[ti]))
waypoints.append(last)
return waypoints
if f > g_score.get(current, float("inf")) + _dist_xyz(centroids[current], end_centroid):
continue
cc = centroids[current]
for neighbor in self._adjacency.get(current, ()):
if neighbor in self._blocked:
continue
nc = centroids[neighbor]
tentative_g = g_score[current] + _dist_xyz(cc, nc)
if tentative_g < g_score.get(neighbor, float("inf")):
came_from[neighbor] = current
g_score[neighbor] = tentative_g
h = _dist_xyz(nc, end_centroid)
heapq.heappush(open_set, (tentative_g + h, neighbor, current))
return [] # No path found
# -- Spatial queries --
[docs]
def get_closest_point(self, point: Vec3, max_distance: float) -> Vec3 | None:
"""Snap a point to the nearest walkable navmesh surface.
Triangles carved away by :meth:`add_obstacle` are not walkable and are
never returned, so the result is always somewhere an agent may stand.
Args:
point: Query point in world space.
max_distance: How far the surface may be from ``point`` and still
count as a result.
Returns:
The closest point on a walkable triangle, or None when the mesh is
empty, fully carved away, or has no walkable surface that near.
"""
self._build_adjacency()
tri_idx = self._find_closest_triangle(point, max_distance)
return None if tri_idx is None else self._closest_point_on_triangle(point, tri_idx)
[docs]
def is_point_on_mesh(self, point: Vec3, tolerance: float = 0.5) -> bool:
"""Test if a point is on the walkable area.
Args:
point: Query point in world space.
tolerance: Maximum distance from navmesh surface to still count.
Returns:
True if point is within tolerance of a walkable triangle. A point
inside a carved obstacle is not on the mesh.
"""
return self.get_closest_point(point, tolerance) is not None
[docs]
def sample_position(self, center: Vec3, radius: float, max_attempts: int = 30) -> Vec3 | None:
"""Sample a random point near center that lies on the walkable navmesh.
Args:
center: Center of the sampling sphere.
radius: Maximum distance from center.
max_attempts: Number of random samples to try.
Returns:
A random walkable point within radius of center, or None if no
attempt landed on one. Carved obstacles are never sampled.
"""
self._build_adjacency()
if not self._triangles:
return None
for _ in range(max_attempts):
# Random offset in sphere
dx = random.uniform(-radius, radius)
dy = random.uniform(-radius, radius)
dz = random.uniform(-radius, radius)
candidate = Vec3(center.x + dx, center.y + dy, center.z + dz)
# Anything within radius of the centre is within this distance of
# the candidate, so the bound cannot reject an acceptable result.
reach = radius + _vec3_dist(candidate, center)
cp = self.get_closest_point(candidate, reach)
if cp is not None and _vec3_dist(cp, center) <= radius:
return cp
return None
def _find_closest_triangle(self, point: Vec3, max_distance: float = float("inf")) -> int | None:
"""Find the nearest non-blocked triangle to point, or None if none is within max_distance.
Ranking is by true distance to the triangle's surface. Ranking by
centroid distance would be cheaper, but the nearest centroid need not
belong to the nearest surface: a long triangle reaching towards the
point ranks badly and a compact one further off ranks well. Deciding
``max_distance`` on a centroid-ranked winner therefore reports nothing
in range while a triangle in range is sitting there unexamined.
The grid keeps that ranking off the whole mesh. Cells are read outwards
from the one holding the point, and the walk stops once the nearest
unread cell is further away than the best surface found so far: a
triangle filed only in unread cells has its whole XZ footprint beyond
that boundary, so it cannot beat the winner. Which triangle wins is
therefore the same one an exhaustive scan picks, ties included.
"""
self._build_adjacency()
if not self._triangles:
return None
limit_sq = max_distance * max_distance
px, py, pz = point.x, point.y, point.z
p = np.array([px, py, pz], dtype=np.float64)
ix, iz = self._cell_of(px, pz)
nx, nz = self._grid_nx, self._grid_nz
grid = self._grid
best_idx: int | None = None
best_dist_sq = float("inf")
seen: set[int] = set()
for ring in range(max(ix, nx - 1 - ix, iz, nz - 1 - iz) + 1):
if ring:
# Nothing beyond the cells already read can be nearer than this.
reach = self._distance_outside_block(px, pz, ix, iz, ring - 1)
if reach * reach > min(best_dist_sq, limit_sq):
break
for iz_c in range(max(iz - ring, 0), min(iz + ring, nz - 1) + 1):
row = iz_c * nx
if iz_c in (iz - ring, iz + ring):
span: list[int] = list(range(max(ix - ring, 0), min(ix + ring, nx - 1) + 1))
else:
# Interior rows contribute only their two end cells; the
# rest of the row belongs to a smaller ring, already read.
span = [c for c in (ix - ring, ix + ring) if 0 <= c < nx]
for ix_c in span:
for i in grid[row + ix_c]:
if i in seen:
continue
seen.add(i)
if i in self._blocked:
continue
v = self._tri_v[i]
cp = _closest_point_on_triangle_np(p, v[0], v[1], v[2])
d = float(((cp - p) ** 2).sum())
if d > limit_sq:
continue
if d < best_dist_sq or (d == best_dist_sq and best_idx is not None and i < best_idx):
best_dist_sq = d
best_idx = i
return best_idx
def _distance_outside_block(self, px: float, pz: float, ix: int, iz: int, ring: int) -> float:
"""Distance in XZ from (px, pz) to the nearest point outside a block of cells.
The block is every cell within ``ring`` cells of (ix, iz), and the
result is zero when the point does not lie inside it.
"""
x_lo = self._grid_x0 + (ix - ring) * self._grid_cw
x_hi = self._grid_x0 + (ix + ring + 1) * self._grid_cw
z_lo = self._grid_z0 + (iz - ring) * self._grid_ch
z_hi = self._grid_z0 + (iz + ring + 1) * self._grid_ch
return max(0.0, min(px - x_lo, x_hi - px, pz - z_lo, z_hi - pz))