1use crate::distance::Matrix;
16use crate::rng::Rng;
17
18#[derive(Clone, Copy, Debug)]
20pub struct RouteLimits {
21 pub max_length: f64,
24 pub max_stops: usize,
26}
27
28impl Default for RouteLimits {
29 fn default() -> Self {
30 RouteLimits {
31 max_length: f64::INFINITY,
32 max_stops: usize::MAX,
33 }
34 }
35}
36
37#[derive(Clone, Copy, Debug)]
39pub struct GraspOptions {
40 pub iterations: usize,
42 pub alpha: f64,
45 pub seed: u64,
47}
48
49impl Default for GraspOptions {
50 fn default() -> Self {
51 GraspOptions {
52 iterations: 200,
53 alpha: 0.3,
54 seed: 1,
55 }
56 }
57}
58
59#[derive(Clone, Debug)]
61pub struct Solution {
62 pub routes: Vec<Vec<usize>>,
64 pub lengths: Vec<f64>,
66 pub total: f64,
68 pub lower_bound: f64,
72 pub best_iteration: usize,
74}
75
76impl Solution {
77 pub fn gap_to_bound(&self) -> f64 {
79 if self.lower_bound <= 0.0 {
80 return f64::NAN;
81 }
82 (self.total - self.lower_bound) / self.lower_bound
83 }
84}
85
86fn validate(m: &Matrix, depot: usize, units: &[usize]) -> Result<(), String> {
87 let n = m.len();
88 if depot >= n {
89 return Err(format!("depot index {depot} is out of range for {n} units"));
90 }
91 if units.is_empty() {
92 return Err("no units to route".into());
93 }
94 let mut seen = vec![false; n];
95 for &u in units {
96 if u >= n {
97 return Err(format!("unit index {u} is out of range for {n} units"));
98 }
99 if u == depot {
100 return Err("the depot cannot be one of the units to visit".into());
101 }
102 if seen[u] {
103 return Err(format!("unit {u} appears twice"));
104 }
105 seen[u] = true;
106 }
107 Ok(())
108}
109
110pub fn route_length(m: &Matrix, depot: usize, route: &[usize]) -> f64 {
112 if route.is_empty() {
113 return 0.0;
114 }
115 let mut total = m.get(depot, route[0]);
116 for w in route.windows(2) {
117 total += m.get(w[0], w[1]);
118 }
119 total + m.get(route[route.len() - 1], depot)
120}
121
122fn two_edge_bound(m: &Matrix, depot: usize, units: &[usize]) -> f64 {
123 let mut nodes: Vec<usize> = Vec::with_capacity(units.len() + 1);
124 nodes.push(depot);
125 nodes.extend_from_slice(units);
126 if nodes.len() < 3 {
127 return if nodes.len() == 2 {
128 2.0 * m.get(nodes[0], nodes[1])
129 } else {
130 0.0
131 };
132 }
133 let mut sum = 0.0;
134 for &i in &nodes {
135 let mut best = [f64::INFINITY; 2];
136 for &j in &nodes {
137 if i == j {
138 continue;
139 }
140 let d = m.get(i, j);
141 if d < best[0] {
142 best[1] = best[0];
143 best[0] = d;
144 } else if d < best[1] {
145 best[1] = d;
146 }
147 }
148 sum += best[0] + best[1];
149 }
150 sum / 2.0
151}
152
153fn construct(m: &Matrix, depot: usize, units: &[usize], alpha: f64, rng: &mut Rng) -> Vec<usize> {
155 let mut remaining: Vec<usize> = units.to_vec();
156 let mut tour = Vec::with_capacity(units.len());
157 let mut current = depot;
158 while !remaining.is_empty() {
159 let costs: Vec<f64> = remaining.iter().map(|&u| m.get(current, u)).collect();
160 let (mut lo, mut hi) = (f64::INFINITY, f64::NEG_INFINITY);
161 for &c in &costs {
162 lo = lo.min(c);
163 hi = hi.max(c);
164 }
165 let threshold = lo + alpha * (hi - lo);
166 let candidates: Vec<usize> = (0..remaining.len())
167 .filter(|&k| costs[k] <= threshold)
168 .collect();
169 let pick = candidates[rng.below(candidates.len())];
170 current = remaining.swap_remove(pick);
171 tour.push(current);
172 }
173 tour
174}
175
176fn two_opt(m: &Matrix, depot: usize, tour: &mut [usize]) -> bool {
178 let n = tour.len();
179 if n < 3 {
180 return false;
181 }
182 let node = |tour: &[usize], k: isize| -> usize {
183 if k < 0 || k as usize >= tour.len() {
184 depot
185 } else {
186 tour[k as usize]
187 }
188 };
189 let mut improved_any = false;
190 let mut improved = true;
191 while improved {
192 improved = false;
193 for i in 0..n - 1 {
194 for j in (i + 1)..n {
195 let a = node(tour, i as isize - 1);
196 let b = tour[i];
197 let c = tour[j];
198 let d = node(tour, j as isize + 1);
199 let delta = m.get(a, c) + m.get(b, d) - m.get(a, b) - m.get(c, d);
200 if delta < -1e-12 {
201 tour[i..=j].reverse();
202 improved = true;
203 improved_any = true;
204 }
205 }
206 }
207 }
208 improved_any
209}
210
211fn or_opt(m: &Matrix, depot: usize, tour: &mut Vec<usize>) -> bool {
213 let mut improved_any = false;
214 let mut improved = true;
215 while improved {
216 improved = false;
217 let n = tour.len();
218 'outer: for seg in 1..=3.min(n.saturating_sub(1)) {
219 for i in 0..=(n - seg) {
220 let prev = if i == 0 { depot } else { tour[i - 1] };
221 let next = if i + seg >= n { depot } else { tour[i + seg] };
222 let first = tour[i];
223 let last = tour[i + seg - 1];
224 let removal_gain = m.get(prev, first) + m.get(last, next) - m.get(prev, next);
225 let rest: Vec<usize> = tour
227 .iter()
228 .enumerate()
229 .filter(|(k, _)| *k < i || *k >= i + seg)
230 .map(|(_, &u)| u)
231 .collect();
232 for p in 0..=rest.len() {
233 let a = if p == 0 { depot } else { rest[p - 1] };
234 let b = if p >= rest.len() { depot } else { rest[p] };
235 for &reversed in &[false, true] {
236 let (f, l) = if reversed {
237 (last, first)
238 } else {
239 (first, last)
240 };
241 let insert_cost = m.get(a, f) + m.get(l, b) - m.get(a, b);
242 if insert_cost < removal_gain - 1e-12 {
243 let mut segment: Vec<usize> = tour[i..i + seg].to_vec();
244 if reversed {
245 segment.reverse();
246 }
247 let mut new_tour = rest.clone();
248 for (k, u) in segment.into_iter().enumerate() {
249 new_tour.insert(p + k, u);
250 }
251 *tour = new_tour;
252 improved = true;
253 improved_any = true;
254 continue 'outer;
255 }
256 }
257 }
258 }
259 }
260 }
261 improved_any
262}
263
264fn local_search(m: &Matrix, depot: usize, tour: &mut Vec<usize>) {
265 loop {
266 let a = two_opt(m, depot, tour);
267 let b = or_opt(m, depot, tour);
268 if !a && !b {
269 break;
270 }
271 }
272}
273
274fn split(
277 m: &Matrix,
278 depot: usize,
279 tour: &[usize],
280 limits: &RouteLimits,
281) -> Option<Vec<Vec<usize>>> {
282 let n = tour.len();
283 let mut cost = vec![f64::INFINITY; n + 1];
284 let mut pred = vec![usize::MAX; n + 1];
285 cost[0] = 0.0;
286 for i in 0..n {
287 if !cost[i].is_finite() {
288 continue;
289 }
290 let mut len = 0.0;
291 for j in i..n {
292 if j - i + 1 > limits.max_stops {
293 break;
294 }
295 if j == i {
296 len = m.get(depot, tour[i]);
297 } else {
298 len += m.get(tour[j - 1], tour[j]);
299 }
300 let route_cost = len + m.get(tour[j], depot);
301 if route_cost > limits.max_length {
302 break;
303 }
304 if cost[i] + route_cost < cost[j + 1] {
305 cost[j + 1] = cost[i] + route_cost;
306 pred[j + 1] = i;
307 }
308 }
309 }
310 if !cost[n].is_finite() {
311 return None;
312 }
313 let mut routes = Vec::new();
314 let mut j = n;
315 while j > 0 {
316 let i = pred[j];
317 routes.push(tour[i..j].to_vec());
318 j = i;
319 }
320 routes.reverse();
321 Some(routes)
322}
323
324pub fn grasp_routes(
329 m: &Matrix,
330 depot: usize,
331 units: &[usize],
332 limits: RouteLimits,
333 options: GraspOptions,
334) -> Result<Solution, String> {
335 validate(m, depot, units)?;
336 if !(0.0..=1.0).contains(&options.alpha) {
337 return Err("alpha must be in [0, 1]".into());
338 }
339 if options.iterations == 0 {
340 return Err("iterations must be at least 1".into());
341 }
342 if limits.max_stops == 0 || limits.max_length.is_nan() || limits.max_length <= 0.0 {
343 return Err("route limits must allow at least one stop".into());
344 }
345 let mut rng = Rng::new(options.seed);
346 let lower_bound = two_edge_bound(m, depot, units);
347 let mut best: Option<Solution> = None;
348 for it in 0..options.iterations {
349 let mut tour = construct(m, depot, units, options.alpha, &mut rng);
350 local_search(m, depot, &mut tour);
351 let mut routes = match split(m, depot, &tour, &limits) {
352 Some(r) => r,
353 None => return Err("some unit cannot be served within the route limits".into()),
354 };
355 if routes.len() > 1 {
356 for r in routes.iter_mut() {
357 local_search(m, depot, r);
358 }
359 }
360 let lengths: Vec<f64> = routes.iter().map(|r| route_length(m, depot, r)).collect();
361 let total: f64 = lengths.iter().sum();
362 if best.as_ref().map_or(true, |b| total < b.total - 1e-12) {
363 best = Some(Solution {
364 routes,
365 lengths,
366 total,
367 lower_bound,
368 best_iteration: it + 1,
369 });
370 }
371 }
372 Ok(best.expect("at least one iteration"))
373}
374
375#[cfg(test)]
376mod tests {
377 use super::*;
378 use crate::distance::euclidean_matrix;
379
380 fn square_points() -> Matrix {
381 let x = [0.0, -1.0, 0.0, 1.0, 1.0, 1.0, 0.0, -1.0, -1.0];
383 let y = [0.0, 1.0, 1.0, 1.0, 0.0, -1.0, -1.0, -1.0, 0.0];
384 euclidean_matrix(&x, &y).unwrap()
385 }
386
387 #[test]
388 fn tsp_on_a_square_is_optimal() {
389 let m = square_points();
390 let units: Vec<usize> = (1..9).collect();
391 let s = grasp_routes(
392 &m,
393 0,
394 &units,
395 RouteLimits::default(),
396 GraspOptions {
397 iterations: 50,
398 alpha: 0.3,
399 seed: 3,
400 },
401 )
402 .unwrap();
403 assert_eq!(s.routes.len(), 1);
404 let optimum = 7.0 + 1.0 + 2f64.sqrt();
407 assert!((s.total - optimum).abs() < 1e-9, "total {}", s.total);
408 assert!(s.lower_bound <= s.total + 1e-9);
409 assert_eq!(route_length(&m, 0, &s.routes[0]), s.total);
410 }
411
412 #[test]
413 fn split_respects_limits() {
414 let m = square_points();
415 let units: Vec<usize> = (1..9).collect();
416 let limits = RouteLimits {
417 max_length: f64::INFINITY,
418 max_stops: 3,
419 };
420 let s = grasp_routes(&m, 0, &units, limits, GraspOptions::default()).unwrap();
421 assert!(s.routes.len() >= 3);
422 assert!(s.routes.iter().all(|r| r.len() <= 3));
423 let mut all: Vec<usize> = s.routes.concat();
424 all.sort();
425 assert_eq!(all, units);
426 let limits = RouteLimits {
428 max_length: 3.0,
429 max_stops: usize::MAX,
430 };
431 let s = grasp_routes(&m, 0, &units, limits, GraspOptions::default()).unwrap();
432 assert!(s.lengths.iter().all(|&l| l <= 3.0 + 1e-9));
433 let limits = RouteLimits {
434 max_length: 2.5,
435 max_stops: usize::MAX,
436 };
437 assert!(grasp_routes(&m, 0, &units, limits, GraspOptions::default()).is_err());
438 let limits = RouteLimits {
439 max_length: 1.0,
440 max_stops: usize::MAX,
441 };
442 assert!(grasp_routes(&m, 0, &units, limits, GraspOptions::default()).is_err());
443 }
444
445 #[test]
446 fn invalid_input_is_refused() {
447 let m = square_points();
448 assert!(grasp_routes(
449 &m,
450 0,
451 &[0, 1],
452 RouteLimits::default(),
453 GraspOptions::default()
454 )
455 .is_err());
456 assert!(grasp_routes(
457 &m,
458 0,
459 &[1, 1],
460 RouteLimits::default(),
461 GraspOptions::default()
462 )
463 .is_err());
464 assert!(
465 grasp_routes(&m, 9, &[1], RouteLimits::default(), GraspOptions::default()).is_err()
466 );
467 assert!(grasp_routes(&m, 0, &[], RouteLimits::default(), GraspOptions::default()).is_err());
468 }
469
470 #[test]
471 fn deterministic_for_a_seed() {
472 let m = square_points();
473 let units: Vec<usize> = (1..9).collect();
474 let o = GraspOptions {
475 iterations: 20,
476 alpha: 0.5,
477 seed: 11,
478 };
479 let a = grasp_routes(&m, 0, &units, RouteLimits::default(), o).unwrap();
480 let b = grasp_routes(&m, 0, &units, RouteLimits::default(), o).unwrap();
481 assert_eq!(a.routes, b.routes);
482 }
483}