Add a star for shortest path

factorio-pathfinding/src/belt_finding/mod.rs

@@ -1,4 +1,5 @@
 use crate::examples::HashMapMap;
+use crate::graph::wheighted_graph::shortest_path::a_star;
 use crate::graph::wheighted_graph::WheightedGraph;
 use crate::misc::Map;
 use crate::priority_queue::binary_heap::BinaryHeap;
@@ -107,7 +108,7 @@ impl Problem {
                 let mut weight = 0.0;
 
                 if self.map.get(x, y).blocked {
-                    weight += 100.0;
+                    weight += 10000.0;
                 }
 
                 self.map.get_mut(x, y).weight = weight;
@@ -117,7 +118,7 @@ impl Problem {
         for (i, path) in self.path.iter().enumerate() {
             if i != without {
                 for p in path {
-                    let weight = 10.0;
+                    let weight = 1000.0;
                     let x = p.0.x as usize;
                     let y = p.0.y as usize;
 
@@ -269,7 +270,17 @@ impl Problem {
                 map: &self.map,
                 end: self.end[i],
             };
-            let p = dijkstra::<MapInternal, FastBinaryHeap<_>>(&m, self.start[i], self.end[i]);
+            // let p = dijkstra::<MapInternal, FastBinaryHeap<_>>(&m, self.start[i], self.end[i]);
+            let p = a_star::<MapInternal, FastBinaryHeap<_>, _>(
+                &m,
+                self.start[i],
+                self.end[i],
+                |&(p, _)| {
+                    1.5 * (PositionType::abs_diff(p.x, self.end[i].0.x)
+                        + PositionType::abs_diff(p.y, self.end[i].0.y))
+                        as f64
+                },
+            );
             if let Some(p) = p {
                 self.path[i] = p;
             } else {

factorio-pathfinding/src/graph/wheighted_graph/shortest_path.rs

@@ -116,6 +116,35 @@ where
     Some(result)
 }
 
+struct GraphWrapper<'a, G, F> {
+    g: &'a G,
+    f: F,
+}
+
+impl<G: WheightedGraph, F: Fn(&G::Node) -> f64> WheightedGraph for GraphWrapper<'_, G, F> {
+    type Node = G::Node;
+
+    fn edge(&self, node: &Self::Node, num: usize) -> Option<(Self::Node, f64)> {
+        self.g.edge(node, num).map(|(n, s)| {
+            let s = s - (self.f)(node) + (self.f)(&n);
+            (n, s)
+        })
+    }
+}
+
+pub fn a_star<G, P, F>(graph: &G, start: G::Node, end: G::Node, dist: F) -> Option<Vec<G::Node>>
+where
+    P: PriorityQueue<QueueObject<G::Node>> + Debug,
+    P::Handle: Debug,
+    G::Node: Eq + Hash + Clone + Debug,
+    G: WheightedGraph,
+    F: Fn(&G::Node) -> f64,
+{
+    let g = GraphWrapper { g: graph, f: dist };
+
+    dijkstra::<GraphWrapper<G, F>, P>(&g, start, end)
+}
+
 #[cfg(test)]
 mod test {