1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
//! Polygon query interface for Detour
//!
//! This module provides the PolyQuery trait that matches the C++ dtPolyQuery interface
use glam::Vec3;
use super::{MeshTile, NavMeshQuery, Poly, PolyRef};
/// Provides custom polygon query behavior.
/// Used by NavMeshQuery::query_polygons.
///
/// This trait is equivalent to the C++ dtPolyQuery class.
pub trait PolyQuery {
/// Called for each batch of unique polygons touched by the search area.
/// This can be called multiple times for a single query.
///
/// # Arguments
/// * `tile` - The tile containing the polygons
/// * `polys` - Array of polygons in the batch
/// * `refs` - Array of polygon references corresponding to the polygons
fn process(&mut self, tile: &MeshTile, polys: &[&Poly], refs: &[PolyRef]);
}
/// Collects polygons within a search area.
///
/// This is equivalent to the C++ dtCollectPolysQuery class.
pub struct CollectPolysQuery {
polys: Vec<PolyRef>,
max_polys: usize,
overflow: bool,
}
impl CollectPolysQuery {
/// Creates a new CollectPolysQuery with the specified maximum capacity.
pub fn new(max_polys: usize) -> Self {
Self {
polys: Vec::with_capacity(max_polys.min(1024)), // Cap initial allocation
max_polys,
overflow: false,
}
}
pub fn polys(&self) -> &[PolyRef] {
&self.polys
}
pub fn num_collected(&self) -> usize {
self.polys.len()
}
/// True if more polygons were available than could be collected.
pub fn overflow(&self) -> bool {
self.overflow
}
pub fn clear(&mut self) {
self.polys.clear();
self.overflow = false;
}
}
impl PolyQuery for CollectPolysQuery {
fn process(&mut self, _tile: &MeshTile, _polys: &[&Poly], refs: &[PolyRef]) {
let remaining = self.max_polys.saturating_sub(self.polys.len());
if remaining < refs.len() {
self.overflow = true;
self.polys.extend_from_slice(&refs[..remaining]);
} else {
self.polys.extend_from_slice(refs);
}
}
}
/// Finds the nearest polygon to a point.
///
/// This is equivalent to the C++ dtFindNearestPolyQuery class.
pub struct FindNearestPolyQuery<'a> {
query: &'a NavMeshQuery<'a>,
center: [f32; 3],
nearest_distance_sqr: f32,
nearest_ref: PolyRef,
nearest_point: [f32; 3],
over_poly: bool,
}
impl<'a> FindNearestPolyQuery<'a> {
/// Creates a new FindNearestPolyQuery.
pub fn new(query: &'a NavMeshQuery<'a>, center: &[f32; 3]) -> Self {
Self {
query,
center: *center,
nearest_distance_sqr: f32::MAX,
nearest_ref: PolyRef::new(0),
nearest_point: [0.0; 3],
over_poly: false,
}
}
pub fn nearest_ref(&self) -> PolyRef {
self.nearest_ref
}
pub fn nearest_point(&self) -> &[f32; 3] {
&self.nearest_point
}
/// True if the nearest point is directly over the polygon surface.
pub fn is_over_poly(&self) -> bool {
self.over_poly
}
pub fn nearest_distance_sqr(&self) -> f32 {
self.nearest_distance_sqr
}
}
impl<'a> PolyQuery for FindNearestPolyQuery<'a> {
fn process(&mut self, _tile: &MeshTile, _polys: &[&Poly], refs: &[PolyRef]) {
for &poly_ref in refs {
// Get closest point on this polygon
if let Ok((closest_pt, is_over_poly)) = self
.query
.closest_point_on_poly(poly_ref, Vec3::from(self.center))
{
let closest_arr = closest_pt.to_array();
// Calculate squared distance
let diff = [
self.center[0] - closest_arr[0],
self.center[1] - closest_arr[1],
self.center[2] - closest_arr[2],
];
let d = if is_over_poly {
// If point is directly over polygon and closer than climb height,
// favor that instead of straight line nearest point
let climb_height = 0.25; // TODO: Get from nav mesh params
let height_diff = diff[1].abs() - climb_height;
if height_diff > 0.0 {
height_diff * height_diff
} else {
0.0
}
} else {
// Regular squared distance
diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2]
};
if d < self.nearest_distance_sqr {
self.nearest_distance_sqr = d;
self.nearest_ref = poly_ref;
self.nearest_point = closest_arr;
self.over_poly = is_over_poly;
}
}
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_collect_polys_query() {
let mut query = CollectPolysQuery::new(10);
// Test basic collection
let refs = vec![PolyRef::new(1), PolyRef::new(2), PolyRef::new(3)];
query.process(&MeshTile::default(), &[], &refs);
assert_eq!(query.num_collected(), 3);
assert!(!query.overflow());
assert_eq!(query.polys(), &refs);
// Test overflow
let more_refs: Vec<_> = (4..20).map(|i| PolyRef::new(i)).collect();
query.process(&MeshTile::default(), &[], &more_refs);
assert_eq!(query.num_collected(), 10);
assert!(query.overflow());
}
#[test]
fn test_collect_polys_clear() {
let mut query = CollectPolysQuery::new(10);
let refs = vec![PolyRef::new(1), PolyRef::new(2)];
query.process(&MeshTile::default(), &[], &refs);
assert_eq!(query.num_collected(), 2);
query.clear();
assert_eq!(query.num_collected(), 0);
assert!(!query.overflow());
}
}