From a145c4b70c1c060826b1236400f22d1f29d968a1 Mon Sep 17 00:00:00 2001 From: hal8174 Date: Thu, 21 Mar 2024 23:41:26 +0100 Subject: [PATCH] Add weights for dijkstra. --- src/belt_finding/mod.rs | 55 ++++++++++++++++++++++------------------- 1 file changed, 29 insertions(+), 26 deletions(-) diff --git a/src/belt_finding/mod.rs b/src/belt_finding/mod.rs index a216877..7965997 100644 --- a/src/belt_finding/mod.rs +++ b/src/belt_finding/mod.rs @@ -16,7 +16,7 @@ pub mod conflict_avoidance; #[derive(Default, Clone, Copy)] pub struct Field { pub blocked: bool, - wheight: f64, + weight: f64, } pub struct Problem { @@ -55,36 +55,36 @@ impl Problem { } fn calculate_wheights(&mut self, without: usize) { - // for x in 0..self.map.width { - // for y in 0..self.map.height { - // let mut wheight = 1.0; + for x in 0..self.map.width { + for y in 0..self.map.height { + let mut weight = 0.0; - // if self.map.get(x, y).blocked { - // wheight += 100.0; - // } + if self.map.get(x, y).blocked { + weight += 100.0; + } - // self.map.get_mut(x, y).wheight = wheight; - // } - // } + self.map.get_mut(x, y).weight = weight; + } + } - // for (i, path) in self.path.iter().enumerate() { - // if i != without { - // for p in path { - // let weight = 50.0; - // let x = p.x as usize; - // let y = p.y as usize; + for (i, path) in self.path.iter().enumerate() { + if i != without { + for p in path { + let weight = 0.5; + let x = p.0.x as usize; + let y = p.0.y as usize; - // self.map.get_mut(x, y).wheight += weight; - // } - // } - // } + self.map.get_mut(x, y).weight += weight; + } + } + } // for p in &self.start { - // self.map.get_mut(p.0.x as usize, p.0.y as usize).wheight = f64::INFINITY; + // self.map.get_mut(p.0.x as usize, p.0.y as usize).weight = f64::INFINITY; // } // for p in &self.end { - // self.map.get_mut(p.0.x as usize, p.0.y as usize).wheight = f64::INFINITY; + // self.map.get_mut(p.0.x as usize, p.0.y as usize).weight = f64::INFINITY; // } } @@ -173,10 +173,12 @@ impl<'a> WheightedGraph for MapInternal<'a> { if self.map.get(next.x as usize, next.y as usize).blocked && self.end != (next, node.1) { return None; } + + let penalty = self.map.get(next.x as usize, next.y as usize).weight; match num { - 0 => Some(((next, node.1), 1.5)), - 1 => Some(((next, node.1.counter_clockwise()), 1.5)), - 2 => Some(((next, node.1.clockwise()), 1.5)), + 0 => Some(((next, node.1), 1.5 + penalty)), + 1 => Some(((next, node.1.counter_clockwise()), 1.5 + penalty)), + 2 => Some(((next, node.1.clockwise()), 1.5 + penalty)), _ => { let mut count = 2; for l in 2..=6 { @@ -195,7 +197,8 @@ impl<'a> WheightedGraph for MapInternal<'a> { if !self.map.get(n.x as usize, n.y as usize).blocked { count += 1; if count == num { - return Some(((n, node.1), 17.5)); + let penalty = penalty + self.map.get(n.x as usize, n.y as usize).weight; + return Some(((n, node.1), 17.5 + penalty)); } } }