proof_engine/editor/
nav_mesh.rs

1
2//! Navigation mesh editor — mesh generation, agent settings, dynamic obstacles, path visualization.
3
4use glam::{Vec2, Vec3};
5use std::collections::{HashMap, BinaryHeap, HashSet};
6use std::cmp::Ordering;
7
8// ---------------------------------------------------------------------------
9// NavMesh geometry
10// ---------------------------------------------------------------------------
11
12#[derive(Debug, Clone, Copy)]
13pub struct NavTriangle {
14    pub vertices: [usize; 3],
15    pub neighbors: [Option<usize>; 3], // adjacent triangle per edge
16    pub area_type: u8,
17    pub flags: u32,
18}
19
20impl NavTriangle {
21    pub fn center(&self, verts: &[Vec3]) -> Vec3 {
22        let a = verts[self.vertices[0]];
23        let b = verts[self.vertices[1]];
24        let c = verts[self.vertices[2]];
25        (a + b + c) / 3.0
26    }
27
28    pub fn normal(&self, verts: &[Vec3]) -> Vec3 {
29        let a = verts[self.vertices[0]];
30        let b = verts[self.vertices[1]];
31        let c = verts[self.vertices[2]];
32        (b - a).cross(c - a).normalize()
33    }
34
35    pub fn area(&self, verts: &[Vec3]) -> f32 {
36        let a = verts[self.vertices[0]];
37        let b = verts[self.vertices[1]];
38        let c = verts[self.vertices[2]];
39        (b - a).cross(c - a).length() * 0.5
40    }
41
42    pub fn contains_point_2d(&self, verts: &[Vec3], p: Vec2) -> bool {
43        let a = Vec2::new(verts[self.vertices[0]].x, verts[self.vertices[0]].z);
44        let b = Vec2::new(verts[self.vertices[1]].x, verts[self.vertices[1]].z);
45        let c = Vec2::new(verts[self.vertices[2]].x, verts[self.vertices[2]].z);
46        let d1 = sign(p, a, b);
47        let d2 = sign(p, b, c);
48        let d3 = sign(p, c, a);
49        let has_neg = (d1 < 0.0) || (d2 < 0.0) || (d3 < 0.0);
50        let has_pos = (d1 > 0.0) || (d2 > 0.0) || (d3 > 0.0);
51        !(has_neg && has_pos)
52    }
53}
54
55fn sign(p1: Vec2, p2: Vec2, p3: Vec2) -> f32 {
56    (p1.x - p3.x) * (p2.y - p3.y) - (p2.x - p3.x) * (p1.y - p3.y)
57}
58
59// ---------------------------------------------------------------------------
60// Area types
61// ---------------------------------------------------------------------------
62
63#[derive(Debug, Clone)]
64pub struct AreaType {
65    pub id: u8,
66    pub name: String,
67    pub cost: f32,
68    pub color: [u8; 4],
69    pub passable: bool,
70}
71
72impl AreaType {
73    pub fn walkable() -> Self { Self { id: 0, name: "Walkable".into(), cost: 1.0, color: [0, 180, 0, 180], passable: true } }
74    pub fn water() -> Self { Self { id: 1, name: "Water".into(), cost: 3.0, color: [0, 100, 200, 180], passable: true } }
75    pub fn mud() -> Self { Self { id: 2, name: "Mud".into(), cost: 2.0, color: [100, 80, 40, 180], passable: true } }
76    pub fn not_walkable() -> Self { Self { id: 3, name: "Not Walkable".into(), cost: f32::INFINITY, color: [200, 0, 0, 180], passable: false } }
77    pub fn jump() -> Self { Self { id: 4, name: "Jump".into(), cost: 1.5, color: [200, 200, 0, 180], passable: true } }
78}
79
80// ---------------------------------------------------------------------------
81// NavMesh
82// ---------------------------------------------------------------------------
83
84#[derive(Debug, Clone)]
85pub struct NavMesh {
86    pub vertices: Vec<Vec3>,
87    pub triangles: Vec<NavTriangle>,
88    pub area_types: Vec<AreaType>,
89    pub bounds_min: Vec3,
90    pub bounds_max: Vec3,
91    pub cell_size: f32,
92    pub cell_height: f32,
93}
94
95impl NavMesh {
96    pub fn new() -> Self {
97        Self {
98            vertices: Vec::new(),
99            triangles: Vec::new(),
100            area_types: vec![
101                AreaType::walkable(),
102                AreaType::water(),
103                AreaType::mud(),
104                AreaType::not_walkable(),
105                AreaType::jump(),
106            ],
107            bounds_min: Vec3::splat(-50.0),
108            bounds_max: Vec3::splat(50.0),
109            cell_size: 0.25,
110            cell_height: 0.2,
111        }
112    }
113
114    pub fn triangle_count(&self) -> usize { self.triangles.len() }
115    pub fn vertex_count(&self) -> usize { self.vertices.len() }
116
117    pub fn find_triangle_at(&self, p: Vec2) -> Option<usize> {
118        self.triangles.iter().enumerate().find(|(_, t)| t.contains_point_2d(&self.vertices, p)).map(|(i, _)| i)
119    }
120
121    pub fn sample_height(&self, p: Vec2) -> Option<f32> {
122        let tri_idx = self.find_triangle_at(p)?;
123        let tri = &self.triangles[tri_idx];
124        // Barycentric interpolation
125        let a = self.vertices[tri.vertices[0]];
126        let b = self.vertices[tri.vertices[1]];
127        let c = self.vertices[tri.vertices[2]];
128        let ap = Vec2::new(p.x - a.x, p.y - a.z);
129        let ab = Vec2::new(b.x - a.x, b.z - a.z);
130        let ac = Vec2::new(c.x - a.x, c.z - a.z);
131        let inv_denom = 1.0 / (ab.x * ac.y - ac.x * ab.y).abs().max(1e-10);
132        let u = (ap.x * ac.y - ac.x * ap.y) * inv_denom;
133        let v = (ab.x * ap.y - ap.x * ab.y) * inv_denom;
134        let w = 1.0 - u - v;
135        Some(a.y * w + b.y * u + c.y * v)
136    }
137
138    /// Generate a simple flat nav mesh for testing.
139    pub fn build_flat(bounds: f32, resolution: u32) -> Self {
140        let mut nm = NavMesh::new();
141        let step = bounds * 2.0 / resolution as f32;
142        let n = (resolution + 1) as usize;
143        for iz in 0..=resolution {
144            for ix in 0..=resolution {
145                let x = -bounds + ix as f32 * step;
146                let z = -bounds + iz as f32 * step;
147                nm.vertices.push(Vec3::new(x, 0.0, z));
148            }
149        }
150        for iz in 0..resolution as usize {
151            for ix in 0..resolution as usize {
152                let base = iz * n + ix;
153                nm.triangles.push(NavTriangle {
154                    vertices: [base, base + 1, base + n],
155                    neighbors: [None, None, None],
156                    area_type: 0,
157                    flags: 0,
158                });
159                nm.triangles.push(NavTriangle {
160                    vertices: [base + 1, base + n + 1, base + n],
161                    neighbors: [None, None, None],
162                    area_type: 0,
163                    flags: 0,
164                });
165            }
166        }
167        nm.bounds_min = Vec3::new(-bounds, -1.0, -bounds);
168        nm.bounds_max = Vec3::new(bounds, 1.0, bounds);
169        nm
170    }
171}
172
173// ---------------------------------------------------------------------------
174// A* pathfinding on triangle mesh
175// ---------------------------------------------------------------------------
176
177#[derive(Debug, Clone)]
178pub struct AStarNode {
179    pub tri_idx: usize,
180    pub g_cost: f32,
181    pub f_cost: f32,
182    pub parent: Option<usize>,
183}
184
185impl PartialEq for AStarNode {
186    fn eq(&self, other: &Self) -> bool { self.tri_idx == other.tri_idx }
187}
188impl Eq for AStarNode {}
189impl PartialOrd for AStarNode {
190    fn partial_cmp(&self, other: &Self) -> Option<Ordering> { Some(self.cmp(other)) }
191}
192impl Ord for AStarNode {
193    fn cmp(&self, other: &Self) -> Ordering {
194        // Reverse for min-heap
195        other.f_cost.partial_cmp(&self.f_cost).unwrap_or(Ordering::Equal)
196    }
197}
198
199#[derive(Debug, Clone)]
200pub struct NavPath {
201    pub waypoints: Vec<Vec3>,
202    pub total_length: f32,
203    pub status: PathStatus,
204}
205
206#[derive(Debug, Clone, Copy, PartialEq)]
207pub enum PathStatus { Complete, Partial, Invalid }
208
209impl NavPath {
210    pub fn empty() -> Self { Self { waypoints: Vec::new(), total_length: 0.0, status: PathStatus::Invalid } }
211
212    pub fn from_waypoints(points: Vec<Vec3>) -> Self {
213        let length: f32 = points.windows(2).map(|w| w[0].distance(w[1])).sum();
214        Self { waypoints: points, total_length: length, status: PathStatus::Complete }
215    }
216
217    pub fn interpolate(&self, t: f32) -> Option<Vec3> {
218        if self.waypoints.len() < 2 { return self.waypoints.first().copied(); }
219        let target_dist = t.clamp(0.0, 1.0) * self.total_length;
220        let mut walked = 0.0_f32;
221        for window in self.waypoints.windows(2) {
222            let seg_len = window[0].distance(window[1]);
223            if walked + seg_len >= target_dist {
224                let u = (target_dist - walked) / seg_len.max(1e-6);
225                return Some(window[0].lerp(window[1], u));
226            }
227            walked += seg_len;
228        }
229        self.waypoints.last().copied()
230    }
231}
232
233pub fn find_path(mesh: &NavMesh, start: Vec3, goal: Vec3) -> NavPath {
234    let start_tri = mesh.find_triangle_at(Vec2::new(start.x, start.z));
235    let goal_tri = mesh.find_triangle_at(Vec2::new(goal.x, goal.z));
236    let (Some(start_idx), Some(goal_idx)) = (start_tri, goal_tri) else {
237        return NavPath::empty();
238    };
239    if start_idx == goal_idx {
240        return NavPath::from_waypoints(vec![start, goal]);
241    }
242    let goal_center = mesh.triangles[goal_idx].center(&mesh.vertices);
243    let mut open: BinaryHeap<AStarNode> = BinaryHeap::new();
244    let mut closed: HashSet<usize> = HashSet::new();
245    let mut came_from: HashMap<usize, usize> = HashMap::new();
246    let mut g_scores: HashMap<usize, f32> = HashMap::new();
247    g_scores.insert(start_idx, 0.0);
248    open.push(AStarNode {
249        tri_idx: start_idx,
250        g_cost: 0.0,
251        f_cost: start.distance(goal_center),
252        parent: None,
253    });
254    while let Some(current) = open.pop() {
255        if current.tri_idx == goal_idx {
256            // Reconstruct path
257            let mut path_tris = vec![goal_idx];
258            let mut cur = goal_idx;
259            while let Some(&prev) = came_from.get(&cur) {
260                path_tris.push(prev);
261                cur = prev;
262            }
263            path_tris.reverse();
264            let waypoints: Vec<Vec3> = path_tris.iter().map(|&i| mesh.triangles[i].center(&mesh.vertices)).collect();
265            let mut wps = vec![start];
266            wps.extend_from_slice(&waypoints[1..]);
267            wps.push(goal);
268            return NavPath::from_waypoints(wps);
269        }
270        closed.insert(current.tri_idx);
271        let neighbors: Vec<usize> = mesh.triangles[current.tri_idx].neighbors.iter()
272            .filter_map(|&n| n).collect();
273        for nb_idx in neighbors {
274            if closed.contains(&nb_idx) { continue; }
275            let tri = &mesh.triangles[nb_idx];
276            let area_cost = mesh.area_types.get(tri.area_type as usize).map(|a| a.cost).unwrap_or(1.0);
277            if area_cost.is_infinite() { continue; }
278            let nb_center = tri.center(&mesh.vertices);
279            let cur_center = mesh.triangles[current.tri_idx].center(&mesh.vertices);
280            let g = g_scores.get(&current.tri_idx).copied().unwrap_or(f32::INFINITY)
281                + cur_center.distance(nb_center) * area_cost;
282            if g < g_scores.get(&nb_idx).copied().unwrap_or(f32::INFINITY) {
283                g_scores.insert(nb_idx, g);
284                came_from.insert(nb_idx, current.tri_idx);
285                let h = nb_center.distance(goal_center);
286                open.push(AStarNode { tri_idx: nb_idx, g_cost: g, f_cost: g + h, parent: Some(current.tri_idx) });
287            }
288        }
289    }
290    NavPath::empty()
291}
292
293// ---------------------------------------------------------------------------
294// NavMesh agent settings
295// ---------------------------------------------------------------------------
296
297#[derive(Debug, Clone)]
298pub struct NavMeshAgentSettings {
299    pub name: String,
300    pub radius: f32,
301    pub height: f32,
302    pub step_height: f32,
303    pub max_slope: f32,
304    pub speed: f32,
305    pub angular_speed: f32,
306    pub acceleration: f32,
307    pub stopping_distance: f32,
308    pub auto_traverse_off_mesh: bool,
309    pub auto_repath: bool,
310    pub auto_brake: bool,
311    pub obstacle_avoidance: ObstacleAvoidanceQuality,
312    pub avoidance_priority: i32,
313    pub area_mask: u32,
314}
315
316#[derive(Debug, Clone, Copy, PartialEq)]
317pub enum ObstacleAvoidanceQuality { None, LowQuality, MedQuality, GoodQuality, HighQuality }
318
319impl Default for NavMeshAgentSettings {
320    fn default() -> Self {
321        Self {
322            name: "Humanoid".into(),
323            radius: 0.4,
324            height: 1.8,
325            step_height: 0.4,
326            max_slope: 45.0,
327            speed: 3.5,
328            angular_speed: 120.0,
329            acceleration: 8.0,
330            stopping_distance: 0.0,
331            auto_traverse_off_mesh: true,
332            auto_repath: true,
333            auto_brake: true,
334            obstacle_avoidance: ObstacleAvoidanceQuality::HighQuality,
335            avoidance_priority: 50,
336            area_mask: 0xFFFF_FFFF,
337        }
338    }
339}
340
341// ---------------------------------------------------------------------------
342// NavMesh build settings
343// ---------------------------------------------------------------------------
344
345#[derive(Debug, Clone)]
346pub struct NavMeshBuildSettings {
347    pub cell_size: f32,
348    pub cell_height: f32,
349    pub min_region_area: f32,
350    pub merge_region_area: f32,
351    pub edge_max_length: f32,
352    pub edge_max_error: f32,
353    pub verts_per_poly: u32,
354    pub detail_sample_distance: f32,
355    pub detail_sample_max_error: f32,
356    pub walkable_slope: f32,
357    pub walkable_height: f32,
358    pub walkable_radius: f32,
359    pub walkable_climb: f32,
360    pub filter_low_hanging: bool,
361    pub filter_ledge_spans: bool,
362    pub filter_walkable_low: bool,
363    pub include_static_objects: bool,
364    pub include_dynamic_objects: bool,
365}
366
367impl Default for NavMeshBuildSettings {
368    fn default() -> Self {
369        Self {
370            cell_size: 0.167,
371            cell_height: 0.1,
372            min_region_area: 2.0,
373            merge_region_area: 400.0,
374            edge_max_length: 12.0,
375            edge_max_error: 1.3,
376            verts_per_poly: 6,
377            detail_sample_distance: 6.0,
378            detail_sample_max_error: 1.0,
379            walkable_slope: 45.0,
380            walkable_height: 2.0,
381            walkable_radius: 0.4,
382            walkable_climb: 0.5,
383            filter_low_hanging: true,
384            filter_ledge_spans: true,
385            filter_walkable_low: true,
386            include_static_objects: true,
387            include_dynamic_objects: false,
388        }
389    }
390}
391
392// ---------------------------------------------------------------------------
393// Off-mesh connections
394// ---------------------------------------------------------------------------
395
396#[derive(Debug, Clone, Copy, PartialEq)]
397pub enum OffMeshConnectionType { Jump, Drop, Ladder, Teleport, Custom }
398
399#[derive(Debug, Clone)]
400pub struct OffMeshConnection {
401    pub id: u32,
402    pub start: Vec3,
403    pub end: Vec3,
404    pub radius: f32,
405    pub bidirectional: bool,
406    pub connection_type: OffMeshConnectionType,
407    pub cost: f32,
408    pub area_mask: u32,
409    pub activated: bool,
410}
411
412impl OffMeshConnection {
413    pub fn new_jump(id: u32, start: Vec3, end: Vec3) -> Self {
414        Self {
415            id, start, end, radius: 0.5, bidirectional: false,
416            connection_type: OffMeshConnectionType::Jump, cost: 1.0,
417            area_mask: 0xFFFF_FFFF, activated: true,
418        }
419    }
420}
421
422// ---------------------------------------------------------------------------
423// NavMesh editor
424// ---------------------------------------------------------------------------
425
426#[derive(Debug, Clone, Copy, PartialEq)]
427pub enum NavMeshEditorTab { Build, Areas, Agents, OffMeshLinks, Visualization, Debug }
428
429#[derive(Debug, Clone, Copy, PartialEq)]
430pub enum NavMeshVisualizationMode { None, Triangles, Walkable, Areas, Regions, Portals }
431
432#[derive(Debug, Clone)]
433pub struct NavMeshEditor {
434    pub mesh: Option<NavMesh>,
435    pub build_settings: NavMeshBuildSettings,
436    pub agent_settings: Vec<NavMeshAgentSettings>,
437    pub off_mesh_connections: Vec<OffMeshConnection>,
438    pub active_tab: NavMeshEditorTab,
439    pub vis_mode: NavMeshVisualizationMode,
440    pub show_agent_cylinder: bool,
441    pub selected_area_type: u8,
442    pub build_progress: f32,
443    pub is_building: bool,
444    pub last_build_time_ms: f32,
445    pub debug_path: Option<NavPath>,
446    pub debug_path_start: Vec3,
447    pub debug_path_goal: Vec3,
448    pub show_debug_path: bool,
449    pub next_connection_id: u32,
450}
451
452impl NavMeshEditor {
453    pub fn new() -> Self {
454        let mut ed = Self {
455            mesh: None,
456            build_settings: NavMeshBuildSettings::default(),
457            agent_settings: vec![NavMeshAgentSettings::default()],
458            off_mesh_connections: Vec::new(),
459            active_tab: NavMeshEditorTab::Build,
460            vis_mode: NavMeshVisualizationMode::Triangles,
461            show_agent_cylinder: true,
462            selected_area_type: 0,
463            build_progress: 0.0,
464            is_building: false,
465            last_build_time_ms: 0.0,
466            debug_path: None,
467            debug_path_start: Vec3::new(-5.0, 0.0, -5.0),
468            debug_path_goal: Vec3::new(5.0, 0.0, 5.0),
469            show_debug_path: false,
470            next_connection_id: 1,
471        };
472        // Build a default flat mesh
473        ed.mesh = Some(NavMesh::build_flat(20.0, 10));
474        ed
475    }
476
477    pub fn start_build(&mut self) {
478        self.is_building = true;
479        self.build_progress = 0.0;
480    }
481
482    pub fn update(&mut self, dt: f32) {
483        if self.is_building {
484            self.build_progress += dt * 0.5;
485            if self.build_progress >= 1.0 {
486                self.build_progress = 1.0;
487                self.is_building = false;
488                self.last_build_time_ms = 1000.0 / 0.5;
489                // Build a synthetic nav mesh
490                self.mesh = Some(NavMesh::build_flat(20.0, 20));
491            }
492        }
493    }
494
495    pub fn compute_debug_path(&mut self) {
496        if let Some(mesh) = &self.mesh {
497            let path = find_path(mesh, self.debug_path_start, self.debug_path_goal);
498            self.debug_path = Some(path);
499        }
500    }
501
502    pub fn add_off_mesh_connection(&mut self, conn: OffMeshConnection) {
503        self.next_connection_id += 1;
504        self.off_mesh_connections.push(conn);
505    }
506
507    pub fn remove_off_mesh_connection(&mut self, id: u32) {
508        self.off_mesh_connections.retain(|c| c.id != id);
509    }
510
511    pub fn triangle_count(&self) -> usize {
512        self.mesh.as_ref().map(|m| m.triangle_count()).unwrap_or(0)
513    }
514
515    pub fn vertex_count(&self) -> usize {
516        self.mesh.as_ref().map(|m| m.vertex_count()).unwrap_or(0)
517    }
518}
519
520// ---------------------------------------------------------------------------
521// Tests
522// ---------------------------------------------------------------------------
523#[cfg(test)]
524mod tests {
525    use super::*;
526
527    #[test]
528    fn test_flat_navmesh() {
529        let nm = NavMesh::build_flat(10.0, 5);
530        assert!(!nm.vertices.is_empty());
531        assert!(!nm.triangles.is_empty());
532    }
533
534    #[test]
535    fn test_point_in_triangle() {
536        let verts = vec![
537            Vec3::new(0.0, 0.0, 0.0),
538            Vec3::new(2.0, 0.0, 0.0),
539            Vec3::new(0.0, 0.0, 2.0),
540        ];
541        let tri = NavTriangle { vertices: [0, 1, 2], neighbors: [None; 3], area_type: 0, flags: 0 };
542        assert!(tri.contains_point_2d(&verts, Vec2::new(0.5, 0.5)));
543        assert!(!tri.contains_point_2d(&verts, Vec2::new(5.0, 5.0)));
544    }
545
546    #[test]
547    fn test_nav_path_interpolation() {
548        let path = NavPath::from_waypoints(vec![Vec3::ZERO, Vec3::X * 10.0]);
549        let mid = path.interpolate(0.5).unwrap();
550        assert!((mid.x - 5.0).abs() < 0.01);
551    }
552
553    #[test]
554    fn test_editor() {
555        let mut ed = NavMeshEditor::new();
556        ed.start_build();
557        ed.update(3.0);
558        assert!(!ed.is_building);
559        assert!(ed.triangle_count() > 0);
560    }
561}
proof_engine/editor/nav_mesh.rs

proof_engine/editor/
nav_mesh.rs
