Multi Path

src/belt_finding/mod.rs

@@ -2,7 +2,7 @@ use crate::graph::wheighted_graph::shortest_path::dijkstra;
 use crate::graph::wheighted_graph::WheightedGraph;
 use crate::priority_queue::BinaryHeap;
 use crate::{misc::Map, priority_queue::PriorityQueue};
-use colored::Colorize;
+use colored::{Color, Colorize};
 use std::fmt::{write, Display};
 use std::io::Write;
 
@@ -28,30 +28,84 @@ impl Position {
 #[derive(Default, Clone, Copy)]
 pub struct Field {
     pub blocked: bool,
+    wheight: f64,
 }
 
 pub struct Problem {
     map: Map<Field>,
-    start: Position,
-    end: Position,
-    path: Vec<Position>,
+    start: Vec<Position>,
+    end: Vec<Position>,
+    path: Vec<Vec<Position>>,
 }
 
 impl Problem {
-    pub fn new(width: usize, height: usize, start: Position, end: Position) -> Self {
+    pub fn new(width: usize, height: usize) -> Self {
         Self {
             map: Map::new(width, height),
-            start,
-            end,
+            start: Vec::new(),
+            end: Vec::new(),
             path: Vec::new(),
         }
     }
 
+    pub fn add_connection(&mut self, start: Position, end: Position) {
+        self.start.push(start);
+        self.end.push(end);
+        self.path.push(Vec::new());
+    }
+
     pub fn set_blocked(&mut self, x: usize, y: usize, blocked: bool) {
         self.map.get_mut(x, y).blocked = blocked;
     }
+
+    pub fn set_blocked_range(&mut self, x1: usize, y1: usize, x2: usize, y2: usize, blocked: bool) {
+        for x in x1..=x2 {
+            for y in y1..=y2 {
+                self.set_blocked(x, y, blocked);
+            }
+        }
+    }
+
+    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;
+
+                if self.map.get(x, y).blocked {
+                    wheight += 100.0;
+                }
+
+                self.map.get_mut(x, y).wheight = wheight;
+            }
+        }
+
+        for (i, path) in self.path.iter().enumerate() {
+            if i != without {
+                for p in path {
+                    self.map.get_mut(p.x, p.y).wheight += 50.0;
+                }
+            }
+        }
+
+        for p in &self.start {
+            self.map.get_mut(p.x, p.y).wheight += 200.0;
+        }
+
+        for p in &self.end {
+            self.map.get_mut(p.x, p.y).wheight += 200.0;
+        }
+    }
 }
 
+static COLORS: [Color; 6] = [
+    Color::Red,
+    Color::Green,
+    Color::Yellow,
+    Color::Blue,
+    Color::Magenta,
+    Color::Cyan,
+];
+
 impl Display for Problem {
     fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
         let width_digits = self.map.width.ilog10() + 1;
@@ -83,22 +137,28 @@ impl Display for Problem {
             write!(f, "{:1$}", y, height_digits as usize)?;
 
             for x in 0..self.map.width {
-                if self.start == Position::new(x, y) {
-                    write!(f, "{}", "s".blue())?;
-                } else if self.end == Position::new(x, y) {
-                    write!(f, "{}", "t".blue())?;
-                } else if let Some(p) = self.path.iter().position(|p| p == &Position::new(x, y)) {
-                    if self.path[p].x < self.path[p + 1].x {
-                        write!(f, "{}", "→".blue())?;
-                    } else if self.path[p].x > self.path[p + 1].x {
-                        write!(f, "{}", "←".blue())?;
-                    } else if self.path[p].y < self.path[p + 1].y {
-                        write!(f, "{}", "↓".blue())?;
-                    } else if self.path[p].y > self.path[p + 1].y {
-                        write!(f, "{}", "↑".blue())?;
+                if let Some(i) = self.start.iter().position(|p| p == &Position::new(x, y)) {
+                    write!(f, "{}", "s".color(COLORS[i]))?;
+                } else if let Some(i) = self.end.iter().position(|p| p == &Position::new(x, y)) {
+                    write!(f, "{}", "t".color(COLORS[i]))?;
+                } else if let Some((i, p)) = self.path.iter().enumerate().find_map(|(i, v)| {
+                    v.iter()
+                        .position(|p| p == &Position::new(x, y))
+                        .map(|j| (i, j))
+                }) {
+                    if self.path[i][p].x < self.path[i][p + 1].x {
+                        write!(f, "{}", "→".color(COLORS[i]))?;
+                    } else if self.path[i][p].x > self.path[i][p + 1].x {
+                        write!(f, "{}", "←".color(COLORS[i]))?;
+                    } else if self.path[i][p].y < self.path[i][p + 1].y {
+                        write!(f, "{}", "↓".color(COLORS[i]))?;
+                    } else if self.path[i][p].y > self.path[i][p + 1].y {
+                        write!(f, "{}", "↑".color(COLORS[i]))?;
                     }
                 } else if self.map.get(x, y).blocked {
                     write!(f, "{}", "#")?;
+                } else if x % 8 == 0 || y % 8 == 0 {
+                    write!(f, "∙")?;
                 } else {
                     write!(f, " ")?;
                 }
@@ -143,10 +203,7 @@ impl<'a> WheightedGraph for MapInternal<'a> {
         } else {
             let t = self.get_direction(*node);
 
-            let v = t
-                .iter()
-                .flatten()
-                .filter_map(|&p| (!self.map.get(p.x, p.y).blocked).then_some(p));
+            let v = t.iter().flatten();
 
             v.count()
         }
@@ -157,20 +214,21 @@ impl<'a> WheightedGraph for MapInternal<'a> {
 
         t.iter()
             .flatten()
-            .filter_map(|&p| (!self.map.get(p.x, p.y).blocked).then_some(p))
             .nth(num)
-            .map(|p| (p, 1.0))
+            .map(|&p| (p, self.map.get(p.x, p.y).wheight))
     }
 }
 
 impl Problem {
     pub fn find_path(&mut self) {
-        let m = MapInternal { map: &self.map };
+        for i in 0..self.start.len() {
+            self.calculate_wheights(i);
+            let m = MapInternal { map: &self.map };
+            let p = dijkstra::<MapInternal, BinaryHeap<_>>(&m, self.start[i], self.end[i]);
 
-        let p = dijkstra::<MapInternal, BinaryHeap<_>>(&m, self.start, self.end);
-
-        if let Some(p) = p {
-            self.path = p;
+            if let Some(p) = p {
+                self.path[i] = p;
+            }
         }
     }
 }