berkus
/
topola
mirror of https://codeberg.org/topola/topola.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Implement basic A* routing

This commit is contained in:
Mikolaj Wielgus 2023-08-27 21:14:42 +02:00
parent 270a7e857c
commit 14f6b9f870
6 changed files with 254 additions and 147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

176
src/astar.rs Normal file
View File

@ -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
}

6
src/guide.rs
View File

@ -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
}

136
src/main.rs
View File

@ -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 {

12
src/math.rs
View File

@ -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();

6
src/mesh.rs
View File

@ -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()
}

65
src/router.rs
View File

@ -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(())
}