Implement basic A* routing

src/astar.rs

@@ -0,0 +1,176 @@
+/**
+ *
+ * Copied from petgraph's scored.rs and algo/astar.rs. Renamed the `is_goal: IsGoal` callback to
+ * `reroute: Reroute` and made it pass a reference to `path_tracker` and return a value to be added
+ * to outgoing edge costs.
+ *
+ * Copyright (c) 2015
+ **/
+use std::collections::hash_map::Entry::{Occupied, Vacant};
+use std::collections::{BinaryHeap, HashMap};
+
+use std::hash::Hash;
+
+use petgraph::algo::Measure;
+use petgraph::visit::{EdgeRef, GraphBase, IntoEdges, Visitable};
+
+use std::cmp::Ordering;
+
+#[derive(Copy, Clone, Debug)]
+pub struct MinScored<K, T>(pub K, pub T);
+
+impl<K: PartialOrd, T> PartialEq for MinScored<K, T> {
+    #[inline]
+    fn eq(&self, other: &MinScored<K, T>) -> bool {
+        self.cmp(other) == Ordering::Equal
+    }
+}
+
+impl<K: PartialOrd, T> Eq for MinScored<K, T> {}
+
+impl<K: PartialOrd, T> PartialOrd for MinScored<K, T> {
+    #[inline]
+    fn partial_cmp(&self, other: &MinScored<K, T>) -> Option<Ordering> {
+        Some(self.cmp(other))
+    }
+}
+
+impl<K: PartialOrd, T> Ord for MinScored<K, T> {
+    #[inline]
+    fn cmp(&self, other: &MinScored<K, T>) -> Ordering {
+        let a = &self.0;
+        let b = &other.0;
+        if a == b {
+            Ordering::Equal
+        } else if a < b {
+            Ordering::Greater
+        } else if a > b {
+            Ordering::Less
+        } else if a.ne(a) && b.ne(b) {
+            // these are the NaN cases
+            Ordering::Equal
+        } else if a.ne(a) {
+            // Order NaN less, so that it is last in the MinScore order
+            Ordering::Less
+        } else {
+            Ordering::Greater
+        }
+    }
+}
+
+pub struct PathTracker<G>
+where
+    G: GraphBase,
+    G::NodeId: Eq + Hash,
+{
+    came_from: HashMap<G::NodeId, G::NodeId>,
+}
+
+impl<G> PathTracker<G>
+where
+    G: GraphBase,
+    G::NodeId: Eq + Hash,
+{
+    fn new() -> PathTracker<G> {
+        PathTracker {
+            came_from: HashMap::new(),
+        }
+    }
+
+    fn set_predecessor(&mut self, node: G::NodeId, previous: G::NodeId) {
+        self.came_from.insert(node, previous);
+    }
+
+    fn reconstruct_path_to(&self, last: G::NodeId) -> Vec<G::NodeId> {
+        let mut path = vec![last];
+
+        let mut current = last;
+        while let Some(&previous) = self.came_from.get(&current) {
+            path.push(previous);
+            current = previous;
+        }
+
+        path.reverse();
+
+        path
+    }
+}
+
+pub fn astar<G, F, H, K, Reroute>(
+    graph: G,
+    start: G::NodeId,
+    mut reroute: Reroute,
+    mut edge_cost: F,
+    mut estimate_cost: H,
+) -> Option<(K, Vec<G::NodeId>)>
+where
+    G: IntoEdges + Visitable,
+    Reroute: FnMut(G::NodeId, &PathTracker<G>) -> Option<K>,
+    G::NodeId: Eq + Hash,
+    F: FnMut(G::EdgeRef) -> K,
+    H: FnMut(G::NodeId) -> K,
+    K: Measure + Copy,
+{
+    let mut visit_next = BinaryHeap::new();
+    let mut scores = HashMap::new(); // g-values, cost to reach the node
+    let mut estimate_scores = HashMap::new(); // f-values, cost to reach + estimate cost to goal
+    let mut path_tracker = PathTracker::<G>::new();
+
+    let zero_score = K::default();
+    scores.insert(start, zero_score);
+    visit_next.push(MinScored(estimate_cost(start), start));
+
+    while let Some(MinScored(estimate_score, node)) = visit_next.pop() {
+        match reroute(node, &path_tracker) {
+            None => {
+                let path = path_tracker.reconstruct_path_to(node);
+                let cost = scores[&node];
+                return Some((cost, path));
+            }
+            Some(route_cost) => {
+                // This lookup can be unwrapped without fear of panic since the node was
+                // necessarily scored before adding it to `visit_next`.
+                let node_score = scores[&node];
+
+                match estimate_scores.entry(node) {
+                    Occupied(mut entry) => {
+                        // If the node has already been visited with an equal or lower score than
+                        // now, then we do not need to re-visit it.
+                        if *entry.get() <= estimate_score {
+                            continue;
+                        }
+                        entry.insert(estimate_score);
+                    }
+                    Vacant(entry) => {
+                        entry.insert(estimate_score);
+                    }
+                }
+
+                for edge in graph.edges(node) {
+                    let next = edge.target();
+                    let next_score = node_score + route_cost + edge_cost(edge);
+
+                    match scores.entry(next) {
+                        Occupied(mut entry) => {
+                            // No need to add neighbors that we have already reached through a
+                            // shorter path than now.
+                            if *entry.get() <= next_score {
+                                continue;
+                            }
+                            entry.insert(next_score);
+                        }
+                        Vacant(entry) => {
+                            entry.insert(next_score);
+                        }
+                    }
+
+                    path_tracker.set_predecessor(next, node);
+                    let next_estimate_score = next_score + estimate_cost(next);
+                    visit_next.push(MinScored(next_estimate_score, next));
+                }
+            }
+        }
+    }
+
+    None
+}

src/guide.rs

@@ -56,7 +56,7 @@ impl<'a, 'b> Guide<'a, 'b> {
         width: f64,
     ) -> Line {
         let from_circle = self.head_circle(&head, width);
-        let to_circle = self.dot_circle(around, width + 5.0);
+        let to_circle = self.dot_circle(around, width);
 
         let from_cw = self.head_cw(&head);
         math::tangent_segment(from_circle, from_cw, to_circle, Some(cw))
@@ -84,7 +84,7 @@ impl<'a, 'b> Guide<'a, 'b> {
                 if let Some(inner) = self.layout.primitive(bend).inner() {
                     self.bend_circle(inner, width)
                 } else {
-                    self.dot_circle(self.layout.primitive(bend).core().unwrap(), width + 5.0)
+                    self.dot_circle(self.layout.primitive(bend).core().unwrap(), width)
                 }
             }
             None => Circle {
@@ -102,7 +102,7 @@ impl<'a, 'b> Guide<'a, 'b> {
             .as_bend()
             .unwrap()
             .circle();
-        circle.r += self.rules.ruleset(&self.conditions).clearance.min + 10.0;
+        circle.r += self.rules.ruleset(&self.conditions).clearance.min;
         circle
     }
 

src/main.rs

@@ -9,6 +9,7 @@ macro_rules! dbg_dot {
 
 #[macro_use]
 mod graph;
+mod astar;
 mod bow;
 mod guide;
 mod layout;
@@ -56,55 +57,6 @@ fn main() {
     let mut i = 0;
     let mut router = Router::new();
 
-    /*let index = router.layout.add_dot(DotWeight {net: 0, circle: Circle {pos: (150.5, 80.5).into(), r: 8.0}});
-    //router.draw_seg(index, Point {x: 400.5, y: 350.5}, 6.0);
-
-    let index2 = router.layout.add_dot(DotWeight {net: 0, circle: Circle {pos: (180.5, 150.5).into(), r: 8.0}});
-    let barrier1 = router.layout.add_dot(DotWeight {net: 0, circle: Circle {pos: (90.5, 150.5).into(), r: 8.0}});
-    router.layout.add_seg(index2, barrier1, 16.0);
-
-    let index3 = router.layout.add_dot(DotWeight {net: 0, circle: Circle {pos: (130.5, 250.5).into(), r: 8.0}});
-    let barrier2 = router.layout.add_dot(DotWeight {net: 0, circle: Circle {pos: (190.5, 250.5).into(), r: 8.0}});
-    router.layout.add_seg(index3, barrier2, 16.0);
-
-    let index4 = router.draw_around_dot(index, index2, true, 5.0);
-    let index5 = router.draw_around_dot(index4, index3, false, 5.0);
-
-    let index6 = router.layout.add_dot(DotWeight {net: 0, circle: Circle {pos: (140.5, 300.5).into(), r: 8.0}});
-    let index7 = router.draw_to(index5, index6, 5.0);*/
-
-    /*let dot1 = router.layout.add_dot(DotWeight {net: 0, circle: Circle {pos: (100.5, 150.5).into(), r: 8.0}});
-    let dot2 = router.layout.add_dot(DotWeight {net: 0, circle: Circle {pos: (130.5, 150.5).into(), r: 8.0}});
-    let dot3 = router.layout.add_dot(DotWeight {net: 0, circle: Circle {pos: (160.5, 150.5).into(), r: 8.0}});
-
-    let obstacle_dot1 = router.layout.add_dot(DotWeight {net: 0, circle: Circle {pos: (220.5, 250.5).into(), r: 8.0}});
-    let obstacle_dot2 = router.layout.add_dot(DotWeight {net: 0, circle: Circle {pos: (70.5, 250.5).into(), r: 8.0}});
-    router.layout.add_seg(obstacle_dot1, obstacle_dot2, 16.0);
-
-    let dot4 = router.layout.add_dot(DotWeight {net: 0, circle: Circle {pos: (180.5, 380.5).into(), r: 8.0}});
-    let dot5 = router.layout.add_dot(DotWeight {net: 0, circle: Circle {pos: (220.5, 380.5).into(), r: 8.0}});
-    let dot6 = router.layout.add_dot(DotWeight {net: 0, circle: Circle {pos: (290.5, 380.5).into(), r: 8.0}});
-
-    let head = router.draw_start(dot3);
-    let head = router.draw_around_dot(head, obstacle_dot1, true, 5.0);
-    let dot3_1 = head.dot;
-    let bend3_1 = head.bend.unwrap();
-    router.draw_finish(head, dot4, 5.0);
-
-    let head = router.draw_start(dot2);
-    let head = router.draw_around_dot(head, dot3, true, 5.0);
-    let dot2_1 = head.dot;
-    let bend2_1 = head.bend.unwrap();
-    let head = router.draw_around_bend(head, bend3_1, true, 5.0);
-    let dot2_2 = head.dot;
-    let bend2_2 = head.bend.unwrap();
-    router.draw_finish(head, dot5, 5.0);
-
-    let head = router.draw_start(dot1);
-    let head = router.draw_around_bend(head, bend2_1, true, 5.0);
-    let head = router.draw_around_bend(head, bend2_2, true, 5.0);
-    router.draw_finish(head, dot6, 5.0);*/
-
     let dot1_1 = router
         .layout
         .add_dot(DotWeight {
@@ -230,16 +182,6 @@ fn main() {
             },
         })
         .unwrap();
-    /*let barrier1_dot2 = router
-        .layout.add_dot(DotWeight {
-            net: 10,
-            circle: Circle {
-                pos: (250.5, 700.5).into(),
-                r: 8.0,
-            },
-        })
-        .unwrap();
-    let _ = router.layout.add_seg(barrier1_dot1, barrier1_dot2, 16.0);*/
 
     let barrier2_dot1 = router
         .layout
@@ -261,82 +203,20 @@ fn main() {
             },
         })
         .unwrap();
-    let _ = router.layout.add_seg(
+    /*let _ = router.layout.add_seg(
         barrier2_dot1,
         barrier2_dot2,
         SegWeight {
             net: 20,
             width: 16.0,
         },
-    );
+    );*/
 
-    let head = router.draw_start(dot5);
+    /*let head = router.draw_start(dot5);
     let head = router.draw_around_dot(head, dot6, false, 5.0).unwrap();
-    let _ = router.draw_finish(head, dot7, 5.0);
+    let _ = router.draw_finish(head, dot7, 5.0);*/
 
-    /*render_times(&mut event_pump, &mut canvas, &mut router, None, -1);
+    router.route(dot1_1, dot1_2);
-
-    let head = router.draw_start(dot1_1);
-    let head = router
-        .draw_around_dot(head, barrier1_dot1, true, 5.0)
-        .unwrap();
-
-    render_times(&mut event_pump, &mut canvas, &mut router, None, 50);
-    let head = router
-        .draw_around_dot(head, barrier2_dot1, true, 5.0)
-        .unwrap();
-
-    render_times(&mut event_pump, &mut canvas, &mut router, None, 50);
-    router.draw_finish(head, dot1_2, 5.0).unwrap();
-
-    render_times(&mut event_pump, &mut canvas, &mut router, None, 50);
-
-    let head = router.draw_start(dot2_1);
-    let head = router
-        .squeeze_around_dot(head, barrier1_dot1, true, 5.0)
-        .unwrap();
-
-    render_times(&mut event_pump, &mut canvas, &mut router, None, 50);
-    let head = router
-        .squeeze_around_dot(head, barrier2_dot1, true, 5.0)
-        .unwrap();
-
-    render_times(&mut event_pump, &mut canvas, &mut router, None, 50);
-    let _ = router.draw_finish(head, dot2_2, 5.0);
-
-    render_times(&mut event_pump, &mut canvas, &mut router, None, 50);
-
-    let head = router.draw_start(dot3_1);
-
-    render_times(&mut event_pump, &mut canvas, &mut router, None, 50);
-    let head = router
-        .squeeze_around_dot(head, barrier1_dot1, true, 5.0)
-        .unwrap();
-
-    render_times(&mut event_pump, &mut canvas, &mut router, None, 50);
-    let head = router
-        .squeeze_around_dot(head, barrier2_dot1, true, 5.0)
-        .unwrap();
-
-    render_times(&mut event_pump, &mut canvas, &mut router, None, 50);
-    let _ = router.draw_finish(head, dot3_2, 5.0);
-
-    render_times(&mut event_pump, &mut canvas, &mut router, None, 50);
-
-    let head = router.draw_start(dot4_1);
-
-    render_times(&mut event_pump, &mut canvas, &mut router, None, 50);
-    let head = router
-        .squeeze_around_dot(head, barrier1_dot1, true, 5.0)
-        .unwrap();
-
-    render_times(&mut event_pump, &mut canvas, &mut router, None, 50);
-    let head = router
-        .squeeze_around_dot(head, barrier2_dot1, true, 5.0)
-        .unwrap();
-
-    render_times(&mut event_pump, &mut canvas, &mut router, None, 50);
-    let _ = router.draw_finish(head, dot4_2, 5.0);*/
 
     render_times(&mut event_pump, &mut canvas, &mut router, None, -1);
     render_times(
@@ -435,7 +315,7 @@ fn render_times(
                 );
             }
 
-            for edge in router.routeedges() {
+            /*for edge in router.routeedges() {
                 let _ = canvas.line(
                     edge.0.x() as i16,
                     edge.0.y() as i16,
@@ -443,7 +323,7 @@ fn render_times(
                     edge.1.y() as i16,
                     Color::RGB(250, 250, 250),
                 );
-            }
+            }*/
         });
 
         if let Err(err) = result {

src/math.rs

@@ -153,18 +153,6 @@ pub fn intersect_circles(circle1: &Circle, circle2: &Circle) -> Vec<Point> {
     [p + r, p - r].into()
 }
 
-/*pub fn cast_point_to_segment(p: &Point, segment: &Line) -> Option<Point> {
-    let delta = segment.end_point() - segment.start_point();
-    let rel_p = *p - segment.start_point();
-    let t = delta.dot(rel_p) / delta.dot(delta);
-
-    if t < 0.0 || t > 1.0 {
-        return None;
-    }
-
-    Some(segment.start_point() + delta * t)
-}*/
-
 pub fn intersect_circle_segment(circle: &Circle, segment: &Line) -> Vec<Point> {
     let delta: Point = segment.delta().into();
     let from = segment.start_point();

src/mesh.rs

@@ -18,7 +18,7 @@ struct Vertex {
     y: f64,
 }
 
-#[derive(Clone, Copy, PartialEq)]
+#[derive(Debug, Hash, Clone, Copy, PartialEq, Eq)]
 pub struct VertexIndex {
     handle: FixedVertexHandle,
 }
@@ -63,6 +63,10 @@ impl Mesh {
         Ok(())
     }
 
+    pub fn dot(&self, vertex: VertexIndex) -> DotIndex {
+        self.triangulation.vertex(vertex.handle).as_ref().dot
+    }
+
     pub fn vertex(&self, dot: DotIndex) -> VertexIndex {
         self.dot_to_vertex[dot.index.index()].unwrap()
     }

src/router.rs

@@ -1,15 +1,17 @@
 use geo::geometry::Point;
 use petgraph::visit::{EdgeRef, IntoEdgeReferences};
+use spade::InsertionError;
 use std::cell::{Ref, RefCell};
 use std::rc::Rc;
 
+use crate::astar::astar;
 use crate::graph::{BendIndex, DotIndex, Path, SegIndex, TaggedIndex};
 use crate::graph::{BendWeight, DotWeight, SegWeight, TaggedWeight};
 use crate::guide::Guide;
 use crate::layout::Layout;
 use crate::math;
 use crate::math::Circle;
-use crate::mesh::Mesh;
+use crate::mesh::{Mesh, VertexIndex};
 use crate::rules::{Conditions, Rules};
 use crate::shape::Shape;
 
@@ -19,6 +21,12 @@ pub struct Router {
     rules: Rules,
 }
 
+struct Route {
+    path: Vec<VertexIndex>,
+    head: Head,
+    width: f64,
+}
+
 pub struct Head {
     pub dot: DotIndex,
     pub bend: Option<BendIndex>,
@@ -33,6 +41,59 @@ impl Router {
         }
     }
 
+    pub fn route(&mut self, from: DotIndex, to: DotIndex) -> Result<(), InsertionError> {
+        // XXX: Should we actually store the mesh? May be useful for debugging, but doesn't look
+        // right.
+        self.mesh.triangulate(&self.layout)?;
+
+        let (cost, mesh_path) = astar(
+            &self.mesh,
+            self.mesh.vertex(from),
+            |node, tracker| (node != self.mesh.vertex(to)).then_some(0),
+            |edge| 1,
+            |_| 0,
+        )
+        .unwrap(); // TODO.
+
+        let path: Vec<DotIndex> = mesh_path
+            .iter()
+            .map(|vertex| self.mesh.dot(*vertex))
+            .collect();
+
+        self.route_path(&path[..], 5.0).unwrap(); // TODO.
+
+        Ok(())
+    }
+
+    fn route_path(&mut self, path: &[DotIndex], width: f64) -> Result<(), ()> {
+        let mut route = self.route_start(path[0], width);
+
+        for dot in &path[1..(path.len() - 1)] {
+            route = self.route_step(route, *dot)?;
+        }
+
+        self.route_finish(route, path[path.len() - 1])
+    }
+
+    fn route_start(&mut self, from: DotIndex, width: f64) -> Route {
+        Route {
+            path: vec![],
+            head: self.draw_start(from),
+            width,
+        }
+    }
+
+    fn route_finish(&mut self, route: Route, into: DotIndex) -> Result<(), ()> {
+        self.draw_finish(route.head, into, route.width)?;
+        Ok(())
+    }
+
+    fn route_step(&mut self, mut route: Route, to: DotIndex) -> Result<Route, ()> {
+        route.head = self.draw_around_dot(route.head, to, true, route.width)?;
+        route.path.push(self.mesh.vertex(to));
+        Ok(route)
+    }
+
     pub fn draw_start(&mut self, from: DotIndex) -> Head {
         Head {
             dot: from,
@@ -47,7 +108,6 @@ impl Router {
             self.draw_finish_in_dot(head, into, width)?;
         }
 
-        self.mesh.triangulate(&self.layout);
         Ok(())
     }
 
@@ -282,7 +342,6 @@ impl Router {
             self.reroute_outward(outer)?;
         }
 
-        self.mesh.triangulate(&self.layout);
         Ok(())
     }