1#![allow(dead_code, clippy::too_many_arguments)]
2
3use rand::Rng;
9use std::cmp::Ordering;
10use std::collections::{BinaryHeap, HashMap};
11
12const N_SAMPLE: usize = 500;
14const N_KNN: usize = 10;
15const MAX_EDGE_LEN: f64 = 30.0;
16
17#[derive(Clone)]
19struct Node {
20 x: f64,
21 y: f64,
22 cost: f64,
23 parent: Option<usize>,
24}
25
26impl Node {
27 fn new(x: f64, y: f64) -> Self {
28 Node {
29 x,
30 y,
31 cost: f64::INFINITY,
32 parent: None,
33 }
34 }
35}
36
37#[derive(Clone)]
39struct QueueItem {
40 cost: f64,
41 index: usize,
42}
43
44impl PartialEq for QueueItem {
45 fn eq(&self, other: &Self) -> bool {
46 self.cost == other.cost
47 }
48}
49
50impl Eq for QueueItem {}
51
52impl Ord for QueueItem {
53 fn cmp(&self, other: &Self) -> Ordering {
54 other
55 .cost
56 .partial_cmp(&self.cost)
57 .unwrap_or(Ordering::Equal)
58 }
59}
60
61impl PartialOrd for QueueItem {
62 fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
63 Some(self.cmp(other))
64 }
65}
66
67struct KDTree {
69 points: Vec<(f64, f64)>,
70}
71
72impl KDTree {
73 fn new(points: Vec<(f64, f64)>) -> Self {
74 KDTree { points }
75 }
76
77 fn query_knn(&self, x: f64, y: f64, k: usize) -> Vec<(usize, f64)> {
78 let mut distances: Vec<(usize, f64)> = self
79 .points
80 .iter()
81 .enumerate()
82 .map(|(i, (px, py))| {
83 let d = ((x - px).powi(2) + (y - py).powi(2)).sqrt();
84 (i, d)
85 })
86 .collect();
87
88 distances.sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap());
89 distances.truncate(k + 1);
90 distances
91 }
92
93 fn min_distance(&self, x: f64, y: f64) -> f64 {
94 self.points
95 .iter()
96 .map(|(px, py)| ((x - px).powi(2) + (y - py).powi(2)).sqrt())
97 .fold(f64::INFINITY, f64::min)
98 }
99}
100
101pub struct PRMPlanner {
103 sample_x: Vec<f64>,
104 sample_y: Vec<f64>,
105 road_map: Vec<Vec<usize>>,
106}
107
108impl PRMPlanner {
109 pub fn new(
111 ox: &[f64],
112 oy: &[f64],
113 start: (f64, f64),
114 goal: (f64, f64),
115 robot_radius: f64,
116 ) -> Self {
117 let obstacle_tree = KDTree::new(ox.iter().zip(oy.iter()).map(|(&x, &y)| (x, y)).collect());
118
119 let xs = ox.iter().copied().chain([start.0, goal.0]);
123 let ys = oy.iter().copied().chain([start.1, goal.1]);
124 let (mut min_x, mut max_x) = xs.fold((f64::INFINITY, f64::NEG_INFINITY), |(lo, hi), v| {
125 (lo.min(v), hi.max(v))
126 });
127 let (mut min_y, mut max_y) = ys.fold((f64::INFINITY, f64::NEG_INFINITY), |(lo, hi), v| {
128 (lo.min(v), hi.max(v))
129 });
130 if max_x - min_x < 1.0 {
131 min_x -= 0.5;
132 max_x += 0.5;
133 }
134 if max_y - min_y < 1.0 {
135 min_y -= 0.5;
136 max_y += 0.5;
137 }
138
139 let (sample_x, sample_y) = Self::sample_points(
140 start,
141 goal,
142 min_x,
143 max_x,
144 min_y,
145 max_y,
146 robot_radius,
147 &obstacle_tree,
148 );
149
150 let road_map = Self::generate_road_map(&sample_x, &sample_y, robot_radius, &obstacle_tree);
151
152 PRMPlanner {
153 sample_x,
154 sample_y,
155 road_map,
156 }
157 }
158
159 fn sample_points(
160 start: (f64, f64),
161 goal: (f64, f64),
162 min_x: f64,
163 max_x: f64,
164 min_y: f64,
165 max_y: f64,
166 robot_radius: f64,
167 obstacle_tree: &KDTree,
168 ) -> (Vec<f64>, Vec<f64>) {
169 let mut rng = rand::rng();
170 let mut sample_x = Vec::new();
171 let mut sample_y = Vec::new();
172
173 let mut attempts = 0;
176 while sample_x.len() < N_SAMPLE && attempts < N_SAMPLE * 200 {
177 attempts += 1;
178 let x = rng.random_range(min_x..max_x);
179 let y = rng.random_range(min_y..max_y);
180
181 let min_dist = obstacle_tree.min_distance(x, y);
182 if min_dist > robot_radius {
183 sample_x.push(x);
184 sample_y.push(y);
185 }
186 }
187
188 sample_x.push(start.0);
189 sample_y.push(start.1);
190 sample_x.push(goal.0);
191 sample_y.push(goal.1);
192
193 (sample_x, sample_y)
194 }
195
196 fn generate_road_map(
197 sample_x: &[f64],
198 sample_y: &[f64],
199 robot_radius: f64,
200 obstacle_tree: &KDTree,
201 ) -> Vec<Vec<usize>> {
202 let sample_tree = KDTree::new(
203 sample_x
204 .iter()
205 .zip(sample_y.iter())
206 .map(|(&x, &y)| (x, y))
207 .collect(),
208 );
209
210 let mut road_map: Vec<Vec<usize>> = vec![Vec::new(); sample_x.len()];
211
212 for (i, (&x, &y)) in sample_x.iter().zip(sample_y.iter()).enumerate() {
213 let neighbors = sample_tree.query_knn(x, y, N_KNN);
214
215 for (j, dist) in neighbors {
216 if i == j {
217 continue;
218 }
219
220 if dist > MAX_EDGE_LEN {
221 continue;
222 }
223
224 if !Self::is_collision(x, y, sample_x[j], sample_y[j], robot_radius, obstacle_tree)
225 {
226 road_map[i].push(j);
227 }
228 }
229 }
230
231 road_map
232 }
233
234 fn is_collision(
235 x1: f64,
236 y1: f64,
237 x2: f64,
238 y2: f64,
239 robot_radius: f64,
240 obstacle_tree: &KDTree,
241 ) -> bool {
242 let dx = x2 - x1;
243 let dy = y2 - y1;
244 let d = (dx * dx + dy * dy).sqrt();
245
246 if d == 0.0 {
247 return false;
248 }
249
250 let step = robot_radius.max(0.05);
253 let n_steps = (d / step).ceil() as usize;
254
255 for i in 0..=n_steps {
256 let t = i as f64 / n_steps as f64;
257 let x = x1 + t * dx;
258 let y = y1 + t * dy;
259
260 let min_dist = obstacle_tree.min_distance(x, y);
261 if min_dist <= robot_radius {
262 return true;
263 }
264 }
265
266 false
267 }
268
269 pub fn plan(&self) -> Option<(Vec<f64>, Vec<f64>)> {
271 let n = self.sample_x.len();
272 let start_idx = n - 2;
273 let goal_idx = n - 1;
274
275 let mut nodes: Vec<Node> = self
276 .sample_x
277 .iter()
278 .zip(self.sample_y.iter())
279 .map(|(&x, &y)| Node::new(x, y))
280 .collect();
281
282 nodes[start_idx].cost = 0.0;
283
284 let mut open_set = BinaryHeap::new();
285 open_set.push(QueueItem {
286 cost: 0.0,
287 index: start_idx,
288 });
289
290 let mut closed_set: HashMap<usize, bool> = HashMap::new();
291
292 while let Some(current) = open_set.pop() {
293 if current.index == goal_idx {
294 return Some(self.reconstruct_path(&nodes, goal_idx));
295 }
296
297 if closed_set.contains_key(¤t.index) {
298 continue;
299 }
300 closed_set.insert(current.index, true);
301
302 for &neighbor_idx in &self.road_map[current.index] {
303 if closed_set.contains_key(&neighbor_idx) {
304 continue;
305 }
306
307 let dx = nodes[neighbor_idx].x - nodes[current.index].x;
308 let dy = nodes[neighbor_idx].y - nodes[current.index].y;
309 let edge_cost = (dx * dx + dy * dy).sqrt();
310 let new_cost = nodes[current.index].cost + edge_cost;
311
312 if new_cost < nodes[neighbor_idx].cost {
313 nodes[neighbor_idx].cost = new_cost;
314 nodes[neighbor_idx].parent = Some(current.index);
315 open_set.push(QueueItem {
316 cost: new_cost,
317 index: neighbor_idx,
318 });
319 }
320 }
321 }
322
323 None
324 }
325
326 fn reconstruct_path(&self, nodes: &[Node], goal_idx: usize) -> (Vec<f64>, Vec<f64>) {
327 let mut path_x = Vec::new();
328 let mut path_y = Vec::new();
329
330 let mut current = goal_idx;
331 while let Some(parent) = nodes[current].parent {
332 path_x.push(nodes[current].x);
333 path_y.push(nodes[current].y);
334 current = parent;
335 }
336 path_x.push(nodes[current].x);
337 path_y.push(nodes[current].y);
338
339 path_x.reverse();
340 path_y.reverse();
341
342 (path_x, path_y)
343 }
344
345 pub fn get_samples(&self) -> (&[f64], &[f64]) {
347 (&self.sample_x, &self.sample_y)
348 }
349
350 pub fn get_edges(&self) -> Vec<((f64, f64), (f64, f64))> {
352 let mut edges = Vec::new();
353
354 for (i, neighbors) in self.road_map.iter().enumerate() {
357 for &j in neighbors {
358 if i < j || !self.road_map[j].contains(&i) {
359 edges.push((
360 (self.sample_x[i], self.sample_y[i]),
361 (self.sample_x[j], self.sample_y[j]),
362 ));
363 }
364 }
365 }
366
367 edges
368 }
369}
370
371#[cfg(test)]
372mod tests {
373 use super::*;
374
375 fn walls() -> (Vec<f64>, Vec<f64>) {
376 let (mut ox, mut oy) = (Vec::new(), Vec::new());
377 for i in 0..=20 {
378 let t = i as f64;
379 for (x, y) in [(t, 0.0), (t, 20.0), (0.0, t), (20.0, t)] {
380 ox.push(x);
381 oy.push(y);
382 }
383 }
384 (ox, oy)
385 }
386
387 #[test]
388 fn a_zero_radius_robot_still_builds_a_roadmap() {
389 let (ox, oy) = walls();
390 let prm = PRMPlanner::new(&ox, &oy, (2.0, 2.0), (18.0, 18.0), 0.0);
391 assert!(prm.plan().is_some());
392 }
393
394 #[test]
395 fn degenerate_obstacle_sets_do_not_panic_or_hang() {
396 let ox: Vec<f64> = (0..10).map(f64::from).collect();
398 let oy = vec![0.0; 10];
399 let prm = PRMPlanner::new(&ox, &oy, (1.0, 3.0), (8.0, 3.0), 0.5);
400 assert!(!prm.get_samples().0.is_empty());
401 let prm = PRMPlanner::new(&[], &[], (1.0, 1.0), (5.0, 4.0), 0.5);
402 assert!(!prm.get_samples().0.is_empty());
403 let (ox, oy) = walls();
405 let prm = PRMPlanner::new(&ox, &oy, (10.0, 10.0), (11.0, 10.0), 50.0);
406 assert_eq!(prm.get_samples().0.len(), 2, "only start and goal");
407 }
408
409 #[test]
410 fn every_edge_of_the_planned_path_is_in_the_roadmap() {
411 let (ox, oy) = walls();
412 let prm = PRMPlanner::new(&ox, &oy, (2.0, 2.0), (18.0, 18.0), 1.0);
413 let (px, py) = prm.plan().expect("path");
414 let edges = prm.get_edges();
415 for i in 0..px.len() - 1 {
416 let (a, b) = ((px[i], py[i]), (px[i + 1], py[i + 1]));
417 assert!(
418 edges
419 .iter()
420 .any(|&(u, v)| (u == a && v == b) || (u == b && v == a)),
421 "path edge {a:?} -> {b:?} is not drawn"
422 );
423 }
424 }
425
426 #[test]
427 fn test_prm_creation() {
428 let mut ox = Vec::new();
429 let mut oy = Vec::new();
430 for i in 0..20 {
431 ox.push(i as f64);
432 oy.push(0.0);
433 ox.push(i as f64);
434 oy.push(20.0);
435 ox.push(0.0);
436 oy.push(i as f64);
437 ox.push(20.0);
438 oy.push(i as f64);
439 }
440
441 let prm = PRMPlanner::new(&ox, &oy, (2.0, 2.0), (18.0, 18.0), 2.0);
442 let (sx, sy) = prm.get_samples();
443 assert!(!sx.is_empty());
444 assert_eq!(sx.len(), sy.len());
445 }
446}