proof_engine/ai/
navmesh.rs

1//! Navigation mesh system for polygon-based pathfinding.
2//!
3//! # Architecture
4//! A `NavMesh` is composed of triangles (convex polygons) connected through
5//! shared edges called portals.  A* runs on the triangle adjacency graph, and
6//! the string-pulling funnel algorithm produces a smooth final path.
7//!
8//! # Example
9//! ```rust
10//! use proof_engine::ai::navmesh::{NavMesh, NavMeshAgent};
11//! use glam::Vec2;
12//!
13//! let mut mesh = NavMesh::new();
14//! let v0 = mesh.add_vertex(Vec2::new(0.0, 0.0));
15//! let v1 = mesh.add_vertex(Vec2::new(10.0, 0.0));
16//! let v2 = mesh.add_vertex(Vec2::new(5.0, 10.0));
17//! mesh.add_triangle([v0, v1, v2]);
18//! mesh.build();
19//!
20//! let path = mesh.find_path(Vec2::new(1.0, 1.0), Vec2::new(8.0, 2.0));
21//! println!("{:?}", path);
22//! ```
23
24use glam::Vec2;
25use std::collections::{BinaryHeap, HashMap, HashSet};
26use std::cmp::Ordering;
27
28// ---------------------------------------------------------------------------
29// Core types
30// ---------------------------------------------------------------------------
31
32/// A vertex on the navigation mesh.
33#[derive(Debug, Clone, Copy)]
34pub struct NavVertex {
35    pub pos: Vec2,
36}
37
38impl NavVertex {
39    pub fn new(pos: Vec2) -> Self { NavVertex { pos } }
40}
41
42/// A portal (shared edge) between two triangles.
43#[derive(Debug, Clone, Copy)]
44pub struct Portal {
45    /// The two vertex indices that form the shared edge.
46    pub left: usize,
47    pub right: usize,
48    /// The two triangle indices on each side.
49    pub tri_a: usize,
50    pub tri_b: usize,
51}
52
53impl Portal {
54    pub fn new(left: usize, right: usize, tri_a: usize, tri_b: usize) -> Self {
55        Portal { left, right, tri_a, tri_b }
56    }
57
58    /// Mid-point of the portal edge.
59    pub fn midpoint(&self, vertices: &[NavVertex]) -> Vec2 {
60        (vertices[self.left].pos + vertices[self.right].pos) * 0.5
61    }
62}
63
64/// A triangle (face) in the navigation mesh.
65#[derive(Debug, Clone)]
66pub struct NavTriangle {
67    /// Vertex indices (CCW winding assumed).
68    pub verts: [usize; 3],
69    /// Adjacent triangle indices per edge (None = boundary).
70    pub neighbors: [Option<usize>; 3],
71    /// Pre-computed centroid.
72    pub center: Vec2,
73    /// Pre-computed area.
74    pub area: f32,
75    /// Portal index per edge (None = boundary edge).
76    pub portals: [Option<usize>; 3],
77}
78
79impl NavTriangle {
80    /// Create a triangle; compute center and area lazily later via `compute()`.
81    pub fn new(verts: [usize; 3]) -> Self {
82        NavTriangle {
83            verts,
84            neighbors: [None; 3],
85            center: Vec2::ZERO,
86            area: 0.0,
87            portals: [None; 3],
88        }
89    }
90
91    /// Compute centroid and area from vertex positions.
92    pub fn compute(&mut self, vertices: &[NavVertex]) {
93        let a = vertices[self.verts[0]].pos;
94        let b = vertices[self.verts[1]].pos;
95        let c = vertices[self.verts[2]].pos;
96        self.center = (a + b + c) / 3.0;
97        // Signed area via cross product
98        self.area = ((b - a).perp_dot(c - a) * 0.5).abs();
99    }
100
101    /// Returns the edge (sorted vertex pair) for edge index 0,1,2.
102    pub fn edge(&self, edge_idx: usize) -> (usize, usize) {
103        let i0 = self.verts[edge_idx];
104        let i1 = self.verts[(edge_idx + 1) % 3];
105        if i0 < i1 { (i0, i1) } else { (i1, i0) }
106    }
107}
108
109/// The main navigation mesh.
110#[derive(Debug, Clone, Default)]
111pub struct NavMesh {
112    pub vertices: Vec<NavVertex>,
113    pub triangles: Vec<NavTriangle>,
114    pub portals: Vec<Portal>,
115    built: bool,
116}
117
118// ---------------------------------------------------------------------------
119// BinaryHeap node for A*
120// ---------------------------------------------------------------------------
121#[derive(Debug, Clone)]
122struct TriHeapNode {
123    f: f32,
124    g: f32,
125    idx: usize,
126}
127impl PartialEq for TriHeapNode { fn eq(&self, o: &Self) -> bool { self.f == o.f } }
128impl Eq for TriHeapNode {}
129impl PartialOrd for TriHeapNode {
130    fn partial_cmp(&self, o: &Self) -> Option<Ordering> { Some(self.cmp(o)) }
131}
132impl Ord for TriHeapNode {
133    fn cmp(&self, o: &Self) -> Ordering {
134        o.f.partial_cmp(&self.f).unwrap_or(Ordering::Equal)
135    }
136}
137
138// ---------------------------------------------------------------------------
139// NavMesh implementation
140// ---------------------------------------------------------------------------
141
142impl NavMesh {
143    pub fn new() -> Self { NavMesh::default() }
144
145    /// Add a vertex and return its index.
146    pub fn add_vertex(&mut self, pos: Vec2) -> usize {
147        self.built = false;
148        let idx = self.vertices.len();
149        self.vertices.push(NavVertex::new(pos));
150        idx
151    }
152
153    /// Add a triangle (by vertex indices) and return its index.
154    pub fn add_triangle(&mut self, verts: [usize; 3]) -> usize {
155        self.built = false;
156        let idx = self.triangles.len();
157        self.triangles.push(NavTriangle::new(verts));
158        idx
159    }
160
161    /// Build adjacency, portals, and pre-compute geometry.  Must be called
162    /// after all triangles are added and before pathfinding.
163    pub fn build(&mut self) {
164        // Compute per-triangle geometry
165        for tri in self.triangles.iter_mut() {
166            let a = self.vertices[tri.verts[0]].pos;
167            let b = self.vertices[tri.verts[1]].pos;
168            let c = self.vertices[tri.verts[2]].pos;
169            tri.center = (a + b + c) / 3.0;
170            tri.area   = ((b - a).perp_dot(c - a) * 0.5).abs();
171        }
172
173        // Build edge->triangle map
174        // edge (sorted v0,v1) -> list of (tri_index, edge_index_in_tri)
175        let mut edge_map: HashMap<(usize, usize), Vec<(usize, usize)>> = HashMap::new();
176        for (ti, tri) in self.triangles.iter().enumerate() {
177            for ei in 0..3 {
178                let edge = tri.edge(ei);
179                edge_map.entry(edge).or_default().push((ti, ei));
180            }
181        }
182
183        self.portals.clear();
184        // Reset adjacency
185        for tri in self.triangles.iter_mut() {
186            tri.neighbors = [None; 3];
187            tri.portals   = [None; 3];
188        }
189
190        for (edge, tris) in &edge_map {
191            if tris.len() == 2 {
192                let (ti_a, ei_a) = tris[0];
193                let (ti_b, ei_b) = tris[1];
194                let portal_idx = self.portals.len();
195                self.portals.push(Portal::new(edge.0, edge.1, ti_a, ti_b));
196                self.triangles[ti_a].neighbors[ei_a] = Some(ti_b);
197                self.triangles[ti_b].neighbors[ei_b] = Some(ti_a);
198                self.triangles[ti_a].portals[ei_a] = Some(portal_idx);
199                self.triangles[ti_b].portals[ei_b] = Some(portal_idx);
200            }
201        }
202
203        self.built = true;
204    }
205
206    /// Test whether a 2D point is inside a given triangle.
207    pub fn point_in_triangle(&self, pt: Vec2, tri_idx: usize) -> bool {
208        let tri = &self.triangles[tri_idx];
209        let a = self.vertices[tri.verts[0]].pos;
210        let b = self.vertices[tri.verts[1]].pos;
211        let c = self.vertices[tri.verts[2]].pos;
212        point_in_triangle_coords(pt, a, b, c)
213    }
214
215    /// Find which triangle contains `pos`.  Returns `None` if outside mesh.
216    pub fn find_triangle(&self, pos: Vec2) -> Option<usize> {
217        // Linear scan — acceptable for small meshes; use spatial hash for large ones
218        for (idx, _) in self.triangles.iter().enumerate() {
219            if self.point_in_triangle(pos, idx) {
220                return Some(idx);
221            }
222        }
223        // Fallback: return nearest triangle centroid
224        self.triangles.iter().enumerate()
225            .min_by(|(_, a), (_, b)| {
226                let da = a.center.distance_squared(pos);
227                let db = b.center.distance_squared(pos);
228                da.partial_cmp(&db).unwrap_or(Ordering::Equal)
229            })
230            .map(|(i, _)| i)
231    }
232
233    /// Find a path from `start` to `end` using A* on the triangle graph,
234    /// then refine with the funnel (string-pulling) algorithm.
235    pub fn find_path(&self, start: Vec2, end: Vec2) -> Option<Vec<Vec2>> {
236        if !self.built { return None; }
237        let start_tri = self.find_triangle(start)?;
238        let end_tri   = self.find_triangle(end)?;
239
240        if start_tri == end_tri {
241            return Some(vec![start, end]);
242        }
243
244        // A* over triangles
245        let tri_path = self.astar_triangles(start_tri, end_tri, start, end)?;
246
247        // Funnel algorithm for smooth path
248        Some(self.funnel_path(&tri_path, start, end))
249    }
250
251    /// A* search over triangles. Returns ordered triangle indices.
252    fn astar_triangles(&self, start: usize, end: usize, start_pos: Vec2, end_pos: Vec2) -> Option<Vec<usize>> {
253        let n = self.triangles.len();
254        let mut g: Vec<f32> = vec![f32::INFINITY; n];
255        let mut parent: Vec<Option<usize>> = vec![None; n];
256        let mut open: BinaryHeap<TriHeapNode> = BinaryHeap::new();
257        let mut closed: HashSet<usize> = HashSet::new();
258
259        g[start] = 0.0;
260        let h = self.triangles[start].center.distance(end_pos);
261        open.push(TriHeapNode { f: h, g: 0.0, idx: start });
262
263        while let Some(cur) = open.pop() {
264            if closed.contains(&cur.idx) { continue; }
265            closed.insert(cur.idx);
266
267            if cur.idx == end {
268                let mut path = Vec::new();
269                let mut c = cur.idx;
270                loop {
271                    path.push(c);
272                    match parent[c] {
273                        Some(p) => c = p,
274                        None => break,
275                    }
276                }
277                path.reverse();
278                return Some(path);
279            }
280
281            for &nb in self.triangles[cur.idx].neighbors.iter().flatten() {
282                if closed.contains(&nb) { continue; }
283                let edge_cost = self.triangles[cur.idx].center.distance(self.triangles[nb].center);
284                let tentative = g[cur.idx] + edge_cost;
285                if tentative < g[nb] {
286                    g[nb] = tentative;
287                    parent[nb] = Some(cur.idx);
288                    let h2 = self.triangles[nb].center.distance(end_pos);
289                    open.push(TriHeapNode { f: tentative + h2, g: tentative, idx: nb });
290                }
291            }
292        }
293        None
294    }
295
296    /// Funnel (string-pulling) algorithm.
297    /// Given an ordered list of triangles, produces a smooth path.
298    fn funnel_path(&self, tri_path: &[usize], start: Vec2, end: Vec2) -> Vec<Vec2> {
299        if tri_path.len() <= 1 {
300            return vec![start, end];
301        }
302
303        let mut path = vec![start];
304
305        // Collect portals between consecutive triangles
306        let mut portals: Vec<(Vec2, Vec2)> = Vec::new(); // (left, right) edges
307        portals.push((start, start)); // dummy start portal
308
309        for window in tri_path.windows(2) {
310            let ta = window[0];
311            let tb = window[1];
312            // Find shared edge
313            if let Some((lv, rv)) = self.shared_edge_ordered(ta, tb) {
314                portals.push((lv, rv));
315            }
316        }
317        portals.push((end, end)); // dummy end portal
318
319        // Simplified funnel: apex moves forward when funnel collapses
320        let mut apex = start;
321        let mut left_idx = 0usize;
322        let mut right_idx = 0usize;
323        let mut left_pt  = start;
324        let mut right_pt = start;
325
326        for i in 1..portals.len() {
327            let (new_left, new_right) = portals[i];
328
329            // Update right side
330            if triangle_area_sign(apex, right_pt, new_right) <= 0.0 {
331                if apex == right_pt || triangle_area_sign(apex, left_pt, new_right) > 0.0 {
332                    right_pt = new_right;
333                    right_idx = i;
334                } else {
335                    path.push(left_pt);
336                    apex = left_pt;
337                    left_idx = i;
338                    right_idx = i;
339                    right_pt = apex;
340                    left_pt  = apex;
341                    continue;
342                }
343            }
344
345            // Update left side
346            if triangle_area_sign(apex, left_pt, new_left) >= 0.0 {
347                if apex == left_pt || triangle_area_sign(apex, right_pt, new_left) < 0.0 {
348                    left_pt = new_left;
349                    left_idx = i;
350                } else {
351                    path.push(right_pt);
352                    apex = right_pt;
353                    left_idx = i;
354                    right_idx = i;
355                    left_pt  = apex;
356                    right_pt = apex;
357                    continue;
358                }
359            }
360            let _ = (left_idx, right_idx); // suppress unused warnings
361        }
362
363        path.push(end);
364        path
365    }
366
367    /// Returns the shared edge (left, right) when moving from triangle `a` to `b`.
368    fn shared_edge_ordered(&self, a: usize, b: usize) -> Option<(Vec2, Vec2)> {
369        let ta = &self.triangles[a];
370        let tb = &self.triangles[b];
371        // Find common vertex indices
372        let mut common = Vec::new();
373        for &va in &ta.verts {
374            if tb.verts.contains(&va) {
375                common.push(va);
376            }
377        }
378        if common.len() < 2 { return None; }
379        let lv = self.vertices[common[0]].pos;
380        let rv = self.vertices[common[1]].pos;
381        Some((lv, rv))
382    }
383
384    /// Returns the number of triangles.
385    pub fn triangle_count(&self) -> usize { self.triangles.len() }
386
387    /// Returns the number of portals.
388    pub fn portal_count(&self) -> usize { self.portals.len() }
389
390    /// Check whether a world position is inside the navmesh.
391    pub fn contains(&self, pos: Vec2) -> bool {
392        self.triangles.iter().enumerate().any(|(i, _)| self.point_in_triangle(pos, i))
393    }
394
395    /// Clamp a position to the nearest point inside the navmesh.
396    pub fn clamp_to_mesh(&self, pos: Vec2) -> Vec2 {
397        if self.contains(pos) { return pos; }
398        // Find nearest triangle centroid
399        self.triangles.iter()
400            .map(|t| t.center)
401            .min_by(|a, b| {
402                a.distance_squared(pos).partial_cmp(&b.distance_squared(pos)).unwrap_or(Ordering::Equal)
403            })
404            .unwrap_or(pos)
405    }
406}
407
408// ---------------------------------------------------------------------------
409// Geometric helpers
410// ---------------------------------------------------------------------------
411
412fn point_in_triangle_coords(pt: Vec2, a: Vec2, b: Vec2, c: Vec2) -> bool {
413    let d1 = cross2d(pt - a, b - a);
414    let d2 = cross2d(pt - b, c - b);
415    let d3 = cross2d(pt - c, a - c);
416    let has_neg = (d1 < 0.0) || (d2 < 0.0) || (d3 < 0.0);
417    let has_pos = (d1 > 0.0) || (d2 > 0.0) || (d3 > 0.0);
418    !(has_neg && has_pos)
419}
420
421#[inline]
422fn cross2d(a: Vec2, b: Vec2) -> f32 {
423    a.x * b.y - a.y * b.x
424}
425
426#[inline]
427fn triangle_area_sign(a: Vec2, b: Vec2, c: Vec2) -> f32 {
428    (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x)
429}
430
431// ---------------------------------------------------------------------------
432// NavMesh Builder
433// ---------------------------------------------------------------------------
434
435/// Axis-aligned bounding box obstacle.
436#[derive(Debug, Clone, Copy)]
437pub struct AabbObstacle {
438    pub min: Vec2,
439    pub max: Vec2,
440}
441
442impl AabbObstacle {
443    pub fn new(min: Vec2, max: Vec2) -> Self { AabbObstacle { min, max } }
444
445    pub fn center(&self) -> Vec2 { (self.min + self.max) * 0.5 }
446
447    pub fn contains(&self, pt: Vec2) -> bool {
448        pt.x >= self.min.x && pt.x <= self.max.x &&
449        pt.y >= self.min.y && pt.y <= self.max.y
450    }
451
452    pub fn corners(&self) -> [Vec2; 4] {
453        [
454            self.min,
455            Vec2::new(self.max.x, self.min.y),
456            self.max,
457            Vec2::new(self.min.x, self.max.y),
458        ]
459    }
460}
461
462/// Builds a `NavMesh` around a set of axis-aligned obstacles within a rectangular world.
463///
464/// The builder uses a simple grid-sampling approach: it subdivides the world into
465/// a regular grid, marks cells occupied by obstacles, and triangulates the free cells.
466pub struct NavMeshBuilder {
467    pub world_min: Vec2,
468    pub world_max: Vec2,
469    pub grid_resolution: usize,
470    pub obstacles: Vec<AabbObstacle>,
471    pub agent_radius: f32,
472}
473
474impl NavMeshBuilder {
475    pub fn new(world_min: Vec2, world_max: Vec2, grid_resolution: usize) -> Self {
476        NavMeshBuilder {
477            world_min,
478            world_max,
479            grid_resolution,
480            obstacles: Vec::new(),
481            agent_radius: 0.0,
482        }
483    }
484
485    pub fn add_obstacle(mut self, obs: AabbObstacle) -> Self {
486        self.obstacles.push(obs);
487        self
488    }
489
490    pub fn with_agent_radius(mut self, radius: f32) -> Self {
491        self.agent_radius = radius;
492        self
493    }
494
495    /// Build and return the completed `NavMesh`.
496    pub fn build(self) -> NavMesh {
497        let res = self.grid_resolution.max(2);
498        let step_x = (self.world_max.x - self.world_min.x) / res as f32;
499        let step_y = (self.world_max.y - self.world_min.y) / res as f32;
500        let pad = self.agent_radius;
501
502        // Determine which grid vertices are free
503        // Vertex grid: (res+1) x (res+1)
504        let vw = res + 1;
505        let vh = res + 1;
506        let mut free = vec![true; vw * vh];
507
508        for vy in 0..vh {
509            for vx in 0..vw {
510                let pos = Vec2::new(
511                    self.world_min.x + vx as f32 * step_x,
512                    self.world_min.y + vy as f32 * step_y,
513                );
514                for obs in &self.obstacles {
515                    let padded_min = obs.min - Vec2::splat(pad);
516                    let padded_max = obs.max + Vec2::splat(pad);
517                    if pos.x >= padded_min.x && pos.x <= padded_max.x &&
518                       pos.y >= padded_min.y && pos.y <= padded_max.y {
519                        free[vy * vw + vx] = false;
520                        break;
521                    }
522                }
523            }
524        }
525
526        let mut mesh = NavMesh::new();
527        // Map from grid (vx,vy) to vertex index in mesh
528        let mut vert_map: HashMap<(usize, usize), usize> = HashMap::new();
529
530        let get_or_insert = |vx: usize, vy: usize,
531                              mesh: &mut NavMesh,
532                              vert_map: &mut HashMap<(usize, usize), usize>,
533                              world_min: Vec2, step_x: f32, step_y: f32| -> usize {
534            *vert_map.entry((vx, vy)).or_insert_with(|| {
535                let pos = Vec2::new(
536                    world_min.x + vx as f32 * step_x,
537                    world_min.y + vy as f32 * step_y,
538                );
539                mesh.add_vertex(pos)
540            })
541        };
542
543        // For each grid cell (res x res), emit two triangles if all verts are free
544        for cy in 0..res {
545            for cx in 0..res {
546                let corners = [(cx, cy), (cx+1, cy), (cx+1, cy+1), (cx, cy+1)];
547                let all_free = corners.iter().all(|&(vx, vy)| free[vy * vw + vx]);
548                if !all_free { continue; }
549
550                let v00 = get_or_insert(cx,   cy,   &mut mesh, &mut vert_map, self.world_min, step_x, step_y);
551                let v10 = get_or_insert(cx+1, cy,   &mut mesh, &mut vert_map, self.world_min, step_x, step_y);
552                let v11 = get_or_insert(cx+1, cy+1, &mut mesh, &mut vert_map, self.world_min, step_x, step_y);
553                let v01 = get_or_insert(cx,   cy+1, &mut mesh, &mut vert_map, self.world_min, step_x, step_y);
554
555                mesh.add_triangle([v00, v10, v11]);
556                mesh.add_triangle([v00, v11, v01]);
557            }
558        }
559
560        mesh.build();
561        mesh
562    }
563}
564
565// ---------------------------------------------------------------------------
566// NavMesh Agent
567// ---------------------------------------------------------------------------
568
569/// An agent that navigates the navmesh, following a computed path.
570#[derive(Debug, Clone)]
571pub struct NavMeshAgent {
572    pub position: Vec2,
573    pub velocity: Vec2,
574    pub target: Vec2,
575    pub path: Vec<Vec2>,
576    pub speed: f32,
577    pub radius: f32,
578    pub arrival_threshold: f32,
579    current_waypoint: usize,
580}
581
582impl NavMeshAgent {
583    pub fn new(position: Vec2, speed: f32, radius: f32) -> Self {
584        NavMeshAgent {
585            position,
586            velocity: Vec2::ZERO,
587            target: position,
588            path: Vec::new(),
589            speed,
590            radius,
591            arrival_threshold: 0.1,
592            current_waypoint: 0,
593        }
594    }
595
596    /// Set a new destination and compute a path on the given navmesh.
597    pub fn set_destination(&mut self, dest: Vec2, navmesh: &NavMesh) {
598        self.target = dest;
599        self.current_waypoint = 0;
600        self.path = navmesh.find_path(self.position, dest).unwrap_or_default();
601    }
602
603    /// Update the agent's position by stepping along its path.
604    pub fn update(&mut self, dt: f32, _navmesh: &NavMesh) {
605        if self.is_at_destination() || self.path.is_empty() {
606            self.velocity = Vec2::ZERO;
607            return;
608        }
609
610        let steering = self.steer_toward_path();
611        self.velocity = steering * self.speed;
612        self.position += self.velocity * dt;
613
614        // Advance waypoint index when close enough
615        if self.current_waypoint < self.path.len() {
616            let wp = self.path[self.current_waypoint];
617            if self.position.distance(wp) <= self.arrival_threshold + self.speed * dt {
618                self.current_waypoint += 1;
619            }
620        }
621    }
622
623    /// Returns `true` when the agent has reached its destination.
624    pub fn is_at_destination(&self) -> bool {
625        self.position.distance(self.target) <= self.arrival_threshold
626    }
627
628    /// Compute a normalised steering direction toward the current path waypoint.
629    pub fn steer_toward_path(&self) -> Vec2 {
630        let wp_idx = self.current_waypoint.min(self.path.len().saturating_sub(1));
631        if self.path.is_empty() { return Vec2::ZERO; }
632        let wp = self.path[wp_idx];
633        let to_wp = wp - self.position;
634        let dist = to_wp.length();
635        if dist < 0.0001 { return Vec2::ZERO; }
636        to_wp / dist
637    }
638
639    /// Distance remaining along the current path.
640    pub fn remaining_distance(&self) -> f32 {
641        if self.path.is_empty() { return 0.0; }
642        let start_idx = self.current_waypoint.min(self.path.len().saturating_sub(1));
643        let mut dist = self.position.distance(self.path[start_idx]);
644        for i in start_idx..self.path.len().saturating_sub(1) {
645            dist += self.path[i].distance(self.path[i + 1]);
646        }
647        dist
648    }
649
650    /// Stop the agent and clear its path.
651    pub fn stop(&mut self) {
652        self.path.clear();
653        self.velocity = Vec2::ZERO;
654        self.current_waypoint = 0;
655    }
656
657    /// Warp agent to a new position without pathfinding.
658    pub fn teleport(&mut self, pos: Vec2) {
659        self.position = pos;
660        self.stop();
661    }
662}
663
664// ---------------------------------------------------------------------------
665// Path query helpers
666// ---------------------------------------------------------------------------
667
668/// Batch path query — finds paths for multiple start/end pairs.
669#[derive(Debug, Clone)]
670pub struct BatchPathQuery {
671    pub queries: Vec<(Vec2, Vec2)>,
672}
673
674impl BatchPathQuery {
675    pub fn new() -> Self { BatchPathQuery { queries: Vec::new() } }
676
677    pub fn add(&mut self, start: Vec2, end: Vec2) {
678        self.queries.push((start, end));
679    }
680
681    /// Execute all queries and return results.
682    pub fn execute(&self, navmesh: &NavMesh) -> Vec<Option<Vec<Vec2>>> {
683        self.queries.iter().map(|(s, e)| navmesh.find_path(*s, *e)).collect()
684    }
685}
686
687impl Default for BatchPathQuery {
688    fn default() -> Self { Self::new() }
689}
690
691// ---------------------------------------------------------------------------
692// Spatial hash for fast triangle lookup
693// ---------------------------------------------------------------------------
694
695/// Spatial hash accelerating `find_triangle` for large meshes.
696#[derive(Debug, Clone)]
697pub struct NavMeshSpatialHash {
698    pub cell_size: f32,
699    cells: HashMap<(i32, i32), Vec<usize>>,
700}
701
702impl NavMeshSpatialHash {
703    pub fn build(mesh: &NavMesh, cell_size: f32) -> Self {
704        let mut cells: HashMap<(i32, i32), Vec<usize>> = HashMap::new();
705        for (ti, tri) in mesh.triangles.iter().enumerate() {
706            let cell = Self::hash_pos(tri.center, cell_size);
707            cells.entry(cell).or_default().push(ti);
708        }
709        NavMeshSpatialHash { cell_size, cells }
710    }
711
712    fn hash_pos(pos: Vec2, cell_size: f32) -> (i32, i32) {
713        ((pos.x / cell_size).floor() as i32, (pos.y / cell_size).floor() as i32)
714    }
715
716    /// Return candidate triangle indices near `pos`.
717    pub fn candidates(&self, pos: Vec2) -> Vec<usize> {
718        let (cx, cy) = Self::hash_pos(pos, self.cell_size);
719        let mut result = Vec::new();
720        for dx in -1..=1i32 {
721            for dy in -1..=1i32 {
722                if let Some(tris) = self.cells.get(&(cx + dx, cy + dy)) {
723                    result.extend_from_slice(tris);
724                }
725            }
726        }
727        result
728    }
729
730    /// Find a triangle containing `pos` using the spatial hash.
731    pub fn find_triangle(&self, mesh: &NavMesh, pos: Vec2) -> Option<usize> {
732        for &ti in &self.candidates(pos) {
733            if mesh.point_in_triangle(pos, ti) {
734                return Some(ti);
735            }
736        }
737        None
738    }
739}
740
741// ---------------------------------------------------------------------------
742// Unit tests
743// ---------------------------------------------------------------------------
744
745#[cfg(test)]
746mod tests {
747    use super::*;
748
749    fn simple_mesh() -> NavMesh {
750        // Two adjacent triangles forming a square [0,0]-[2,1]
751        let mut mesh = NavMesh::new();
752        let v0 = mesh.add_vertex(Vec2::new(0.0, 0.0));
753        let v1 = mesh.add_vertex(Vec2::new(2.0, 0.0));
754        let v2 = mesh.add_vertex(Vec2::new(2.0, 1.0));
755        let v3 = mesh.add_vertex(Vec2::new(0.0, 1.0));
756        mesh.add_triangle([v0, v1, v2]);
757        mesh.add_triangle([v0, v2, v3]);
758        mesh.build();
759        mesh
760    }
761
762    #[test]
763    fn test_build_adjacency() {
764        let mesh = simple_mesh();
765        assert_eq!(mesh.portal_count(), 1);
766        assert!(mesh.triangles[0].neighbors.contains(&Some(1)));
767        assert!(mesh.triangles[1].neighbors.contains(&Some(0)));
768    }
769
770    #[test]
771    fn test_point_in_triangle() {
772        let mesh = simple_mesh();
773        assert!(mesh.point_in_triangle(Vec2::new(1.0, 0.3), 0));
774        assert!(!mesh.point_in_triangle(Vec2::new(5.0, 5.0), 0));
775    }
776
777    #[test]
778    fn test_find_triangle() {
779        let mesh = simple_mesh();
780        assert!(mesh.find_triangle(Vec2::new(1.0, 0.3)).is_some());
781        assert!(mesh.find_triangle(Vec2::new(0.5, 0.8)).is_some());
782    }
783
784    #[test]
785    fn test_find_path_same_triangle() {
786        let mesh = simple_mesh();
787        let path = mesh.find_path(Vec2::new(0.5, 0.2), Vec2::new(1.5, 0.4));
788        assert!(path.is_some());
789        let p = path.unwrap();
790        assert_eq!(p[0], Vec2::new(0.5, 0.2));
791    }
792
793    #[test]
794    fn test_find_path_across_triangles() {
795        let mesh = simple_mesh();
796        let path = mesh.find_path(Vec2::new(0.3, 0.2), Vec2::new(1.8, 0.8));
797        assert!(path.is_some());
798    }
799
800    #[test]
801    fn test_navmesh_agent_at_destination() {
802        let mesh = simple_mesh();
803        let mut agent = NavMeshAgent::new(Vec2::new(0.5, 0.4), 2.0, 0.1);
804        agent.set_destination(Vec2::new(1.8, 0.7), &mesh);
805        // Simulate several steps
806        for _ in 0..100 {
807            agent.update(0.016, &mesh);
808            if agent.is_at_destination() { break; }
809        }
810        assert!(agent.is_at_destination() || agent.remaining_distance() < 1.0);
811    }
812
813    #[test]
814    fn test_navmesh_builder_empty() {
815        let mesh = NavMeshBuilder::new(
816            Vec2::new(0.0, 0.0),
817            Vec2::new(10.0, 10.0),
818            4,
819        ).build();
820        assert!(mesh.triangle_count() > 0);
821    }
822
823    #[test]
824    fn test_navmesh_builder_with_obstacle() {
825        let mesh = NavMeshBuilder::new(
826            Vec2::new(0.0, 0.0),
827            Vec2::new(10.0, 10.0),
828            8,
829        )
830        .add_obstacle(AabbObstacle::new(Vec2::new(3.0, 3.0), Vec2::new(7.0, 7.0)))
831        .build();
832        assert!(mesh.triangle_count() > 0);
833        // Obstacle interior should not be in mesh
834        // (may or may not be true depending on grid alignment, so just check build succeeded)
835    }
836
837    #[test]
838    fn test_spatial_hash() {
839        let mesh = simple_mesh();
840        let hash = NavMeshSpatialHash::build(&mesh, 2.0);
841        let tri = hash.find_triangle(&mesh, Vec2::new(1.0, 0.3));
842        assert!(tri.is_some());
843    }
844
845    #[test]
846    fn test_batch_query() {
847        let mesh = simple_mesh();
848        let mut batch = BatchPathQuery::new();
849        batch.add(Vec2::new(0.3, 0.2), Vec2::new(1.8, 0.8));
850        batch.add(Vec2::new(0.5, 0.4), Vec2::new(1.5, 0.6));
851        let results = batch.execute(&mesh);
852        assert_eq!(results.len(), 2);
853    }
854
855    #[test]
856    fn test_contains() {
857        let mesh = simple_mesh();
858        assert!(mesh.contains(Vec2::new(1.0, 0.5)));
859        assert!(!mesh.contains(Vec2::new(-5.0, -5.0)));
860    }
861
862    #[test]
863    fn test_portal_midpoint() {
864        let mesh = simple_mesh();
865        if !mesh.portals.is_empty() {
866            let mid = mesh.portals[0].midpoint(&mesh.vertices);
867            assert!(mid.x > 0.0);
868        }
869    }
870
871    #[test]
872    fn test_remaining_distance() {
873        let mesh = simple_mesh();
874        let mut agent = NavMeshAgent::new(Vec2::new(0.3, 0.2), 1.0, 0.1);
875        agent.set_destination(Vec2::new(1.8, 0.8), &mesh);
876        let dist = agent.remaining_distance();
877        assert!(dist >= 0.0);
878    }
879
880    #[test]
881    fn test_clamp_to_mesh() {
882        let mesh = simple_mesh();
883        let clamped = mesh.clamp_to_mesh(Vec2::new(100.0, 100.0));
884        // Should return some point on the mesh
885        assert!(clamped.x.is_finite());
886    }
887}
proof_engine/ai/navmesh.rs

proof_engine/ai/
navmesh.rs
