diff --git a/src/astar.rs b/src/astar.rs new file mode 100644 index 0000000..6dc0254 --- /dev/null +++ b/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(pub K, pub T); + +impl PartialEq for MinScored { + #[inline] + fn eq(&self, other: &MinScored) -> bool { + self.cmp(other) == Ordering::Equal + } +} + +impl Eq for MinScored {} + +impl PartialOrd for MinScored { + #[inline] + fn partial_cmp(&self, other: &MinScored) -> Option { + Some(self.cmp(other)) + } +} + +impl Ord for MinScored { + #[inline] + fn cmp(&self, other: &MinScored) -> 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 +where + G: GraphBase, + G::NodeId: Eq + Hash, +{ + came_from: HashMap, +} + +impl PathTracker +where + G: GraphBase, + G::NodeId: Eq + Hash, +{ + fn new() -> PathTracker { + 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 { + let mut path = vec![last]; + + let mut current = last; + while let Some(&previous) = self.came_from.get(¤t) { + path.push(previous); + current = previous; + } + + path.reverse(); + + path + } +} + +pub fn astar( + graph: G, + start: G::NodeId, + mut reroute: Reroute, + mut edge_cost: F, + mut estimate_cost: H, +) -> Option<(K, Vec)> +where + G: IntoEdges + Visitable, + Reroute: FnMut(G::NodeId, &PathTracker) -> Option, + 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::::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 +} diff --git a/src/guide.rs b/src/guide.rs index 2887dc3..9d0823c 100644 --- a/src/guide.rs +++ b/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 } diff --git a/src/main.rs b/src/main.rs index 58d3a15..0c56ef5 100644 --- a/src/main.rs +++ b/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); - - 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);*/ + router.route(dot1_1, dot1_2); 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 { diff --git a/src/math.rs b/src/math.rs index 44dd971..1fb39ac 100644 --- a/src/math.rs +++ b/src/math.rs @@ -153,18 +153,6 @@ pub fn intersect_circles(circle1: &Circle, circle2: &Circle) -> Vec { [p + r, p - r].into() } -/*pub fn cast_point_to_segment(p: &Point, segment: &Line) -> Option { - 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 { let delta: Point = segment.delta().into(); let from = segment.start_point(); diff --git a/src/mesh.rs b/src/mesh.rs index be0cbd9..4490adb 100644 --- a/src/mesh.rs +++ b/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() } diff --git a/src/router.rs b/src/router.rs index 0687bbd..93636fe 100644 --- a/src/router.rs +++ b/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, + head: Head, + width: f64, +} + pub struct Head { pub dot: DotIndex, pub bend: Option, @@ -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 = 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.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(()) }