diff --git a/src/paths/pathfinding.jl b/src/paths/pathfinding.jl new file mode 100644 index 00000000..ec14813c --- /dev/null +++ b/src/paths/pathfinding.jl @@ -0,0 +1,134 @@ +import Graphs: a_star, grid, weights, add_edge! +import DeviceLayout: Rectangle, Polygon + +abstract type PathfindingRule{T <: RouteRule} <: RouteRule end + +struct AStarRouting{T, U <: Coordinate} <: PathfindingRule{T} + leg_rule::T + domain::Rectangle{U} + grid_step::U + exclusion_fn::Function + exclusion::Vector{Polygon{U}} + excluded + + function AStarRouting(l, d::Rectangle{U}, s, f::Function) where {U} + return new{typeof(l), U}(l, d, s, f, Polygon{U}[], []) + end +end + +leg_rule(pr::PathfindingRule) = pr.leg_rule + +function _route_leg!( + p::Path, + next::Point, + nextdir, + rule::PathfindingRule, + sty::Paths.Style=Paths.contstyle1(p) +) + # Just use underlying rule + return _route_leg!(p, next, nextdir, leg_rule(rule), sty) +end + +function reconcile!( + path::Path, + endpoint::Point, + end_direction, + rule::AStarRouting, + waypoints, + waydirs; + initialize_waydirs=false +) + # Calculate and insert waypoints + # Construct grid graph + bbox = bounds(rule.domain) + grid_x = range(bbox.ll.x, step=rule.grid_step, stop=bbox.ur.x) + grid_y = range(bbox.ll.y, step=rule.grid_step, stop=bbox.ur.y) + in_poly = DeviceLayout.gridpoints_in_polygon(rule.exclusion, grid_x, grid_y) + nx = length(grid_x) + ny = length(grid_y) + grid_graph = grid((nx, ny)) + + # Functions to convert from real space/cartesian indices to graph indices + v_to_ix_iy(v::Int) = Tuple(CartesianIndices((1:nx, 1:ny))[v]) + ix_iy_to_v(ix::Int, iy::Int) = LinearIndices((1:nx, 1:ny))[ix, iy] + v_to_xy(v::Int) = Point(grid_x[v_to_ix_iy(v)[1]], grid_y[v_to_ix_iy(v)[2]]) + xy_to_v(p::Point) = ix_iy_to_v( + findfirst(xi -> xi + rule.grid_step / 2 > p.x, grid_x), + findfirst(yi -> yi + rule.grid_step / 2 > p.y, grid_y) + ) + + # Set up pathfinding + start_v = xy_to_v(p0(path)) + end_v = xy_to_v(endpoint) + source = xy_to_v(p0(path) + # pathfinding starts with grid box index in front of start + rule.grid_step * Point(cos(α0(path)), sin(α0(path)))) + target = xy_to_v(endpoint - # pathfinding ends with grid box index in front of end + rule.grid_step * Point(cos(end_direction), sin(end_direction))) + heuristic(v) = uconvert(NoUnits, norm(endpoint - v_to_xy(v)) / rule.grid_step) + + w = Matrix{Float64}(weights(grid_graph)) + ## Add diagonals + # for v1 = 1:(nx*ny) + # for v2 = (v1+1):(nx*ny) + # ix1, iy1 = v_to_ix_iy(v1) + # ix2, iy2 = v_to_ix_iy(v2) + # if abs(ix1 - ix2) == 1 && abs(iy1 - iy2) == 1 + # add_edge!(grid_graph, v1, v2) + # w[v1, v2] = w[v2, v1] = sqrt(2) + # end + # end + # end + + ## Set edges to excluded grid cells to infinite weight + ## (deleting vertices would renumber them) + in_poly[start_v] = true + in_poly[end_v] = true + for (v, excluded) in enumerate(in_poly) + !excluded && continue + w[v, :] .= Inf + w[:, v] .= Inf + end + + # Find shortest path + ## Not deterministic! + edges = a_star(grid_graph, source, target, w, heuristic) + # Create waypoints + ## Whenever we change direction, add a waypoint + edge_direction(v1, v2) = (v_to_ix_iy(v2) .- v_to_ix_iy(v1)) + current_direction = edge_direction(start_v, source) + for e in edges + direction = edge_direction(e.src, e.dst) + if direction != current_direction + push!(waypoints, (v_to_xy(e.src) + v_to_xy(e.dst)) / 2) + push!(waydirs, atan(direction[2], direction[1])) + current_direction = direction + end + end + final_direction = edge_direction(target, end_v) + if current_direction == final_direction + pop!(waypoints) # Don't need the last one + pop!(waydirs) + end +end + +function _update_rule!(sch, node, rule::RouteRule) end + +function _update_rule!(sch, node, rule::PathfindingRule) + for planned_node in keys(sch.ref_dict) # Only components already planned + (planned_node in rule.excluded) && continue + append!( + rule.exclusion, + reduce( + vcat, + DeviceLayout.to_polygons.( + transformation( + sch, + planned_node + ).(flatten(rule.exclusion_fn(planned_node.component)).elements) + ), + init=Polygon{coordinatetype(sch)}[] + ) + ) + push!(rule.excluded, planned_node) + end +end diff --git a/src/paths/paths.jl b/src/paths/paths.jl index 1af5b0bd..8aec6f56 100644 --- a/src/paths/paths.jl +++ b/src/paths/paths.jl @@ -690,6 +690,7 @@ include("segments/offset.jl") include("segments/bspline_approximation.jl") include("routes.jl") +include("pathfinding.jl") function change_handedness!(seg::Union{Turn, Corner}) return seg.α = -seg.α diff --git a/src/paths/routes.jl b/src/paths/routes.jl index 7311ff5b..b57af65c 100644 --- a/src/paths/routes.jl +++ b/src/paths/routes.jl @@ -8,7 +8,7 @@ of the following two methods: ``` _route!(p::Path, p1::Point, α1, rule::MyRouteRule, sty, waypoints, waydirs) -_route_leg!(p::Path, next::Point, rule::MyRouteRule, +_route_leg!(p::Path, next::Point, nextdir, rule::MyRouteRule, sty::Paths.Style=Paths.contstyle1(p)) ``` @@ -427,15 +427,15 @@ function route!( initialize_waydirs=initialize_waydirs ) _route!(path, p_end, α_end, rule, sty, waypoints, waydirs) - isapprox(rem(α1(path) - α_end, 360°, RoundNearest), 0, atol=1e-9) || error(""" + isapprox(rem(α1(path) - α_end, 360°, RoundNearest), 0, atol=1e-9) || @error """ Could not automatically route to destination with the correct arrival angle \ (got $(α1(path)) instead of $α_end). Try adding or adjusting waypoints.\ - """) + """ pts = promote(p1(path), p_end) - return isapprox(pts...; atol=atol) || error(""" + return isapprox(pts...; atol=atol) || @error """ Could not automatically route to destination with the correct arrival point \ (got $(p1(path)) instead of $p_end). Try adding or adjusting waypoints.\ - """) + """ end """ diff --git a/src/schematics/schematics.jl b/src/schematics/schematics.jl index 999cdfa5..d75bc2ab 100644 --- a/src/schematics/schematics.jl +++ b/src/schematics/schematics.jl @@ -1041,6 +1041,7 @@ function _plan_route!(sch::Schematic, node_cs, node, hooks_fn=hooks) comp1.r.waypoints = rot.(comp1.r.waypoints) .+ Ref(comp1.r.p0) comp1.r.waydirs = comp1.r.waydirs .+ comp1.r.α0 end + Paths._update_rule!(sch, node, comp1.r.rule) return addref!(node_cs, comp1) end diff --git a/test/test_pathfinding.jl b/test/test_pathfinding.jl new file mode 100644 index 00000000..e61f967a --- /dev/null +++ b/test/test_pathfinding.jl @@ -0,0 +1,39 @@ +using DeviceLayout, .SchematicDrivenLayout, .PreferredUnits +using FileIO + +g = SchematicGraph("test") + +cs = CoordinateSystem("basic") +place!(cs, centered(Rectangle(0.100mm, 0.100mm)), :obstacle) +hooks = (; + port1=PointHook(0.05mm, -0.0mm, 180°), + port2=PointHook(-0.05mm, 0.0mm, 0°), + north=PointHook(0mm, 0.5mm, -90°), + south=PointHook(0mm, -0.5mm, 90°), + west=PointHook(-0.5mm, 0mm, 0°), + east=PointHook(0.5mm, 0mm, 180°) +) +comp = BasicComponent(cs, hooks) +n1 = add_node!(g, comp) +n2 = fuse!(g, n1 => :north, comp => :south) +n3 = fuse!(g, n1 => :south, comp => :north) +n4 = fuse!(g, n1 => :east, comp => :west) +n5 = fuse!(g, n1 => :west, comp => :east) + +rule = Paths.AStarRouting( + Paths.StraightAnd90(min_bend_radius=50μm), + centered(Rectangle(5e6nm, 5e6nm)), + 0.25mm, + make_halo(0.05mm) +) +route!(g, rule, n5 => :port2, n1 => :port1, Paths.CPW(10μm, 6μm), SemanticMeta(:route)) +route!(g, rule, n1 => :port2, n3 => :port2, Paths.CPW(10μm, 6μm), SemanticMeta(:route)) +route!(g, rule, n5 => :port1, n4 => :port2, Paths.CPW(10μm, 6μm), SemanticMeta(:route)) +route!(g, rule, n2 => :port1, n4 => :port1, Paths.CPW(10μm, 6μm), SemanticMeta(:route)) +route!(g, rule, n3 => :port1, n2 => :port2, Paths.CPW(10μm, 6μm), SemanticMeta(:route)) + +@time sch = plan(g; log_dir=nothing, strict=:no) + +c = Cell("test") +render!(c, sch.coordinate_system; map_meta=_ -> GDSMeta()) +save("test.gds", flatten(c; name="flatten"))