Add auto_speed option for BSplines to avoid sharp bends

docs/src/paths.md

@@ -23,7 +23,9 @@ capture an initial and final angle, a radius, and an origin. All circular turns
 parameterized with these variables.
 
 Another useful `Segment` subtype is [`Paths.BSpline`](@ref), which interpolates between two
-or more points with specified start and end tangents.
+or more points with specified start and end tangents (and curvature, optionally) using a [cubic B-spline](https://en.wikipedia.org/wiki/B-spline#Cubic_B-Splines).
+These have the property that curvature is continuous along the spline, and
+can be automatically optimized further to avoid sharp changes in curvature.
 
 ## Styles
 

src/paths/paths.jl

@@ -49,7 +49,9 @@ import DeviceLayout:
     Meta,
     PointHook,
     Polygons,
+    Reflection,
     Rotation,
+    RotationPi,
     ScaledIsometry,
     StructureReference,
     XReflection,
@@ -688,6 +690,7 @@ include("segments/compound.jl")
 include("segments/bspline.jl")
 include("segments/offset.jl")
 include("segments/bspline_approximation.jl")
+include("segments/bspline_optimization.jl")
 
 include("routes.jl")
 

src/paths/routes.jl

@@ -69,6 +69,9 @@ end
 """
     Base.@kwdef struct BSplineRouting <: RouteRule
         endpoints_speed = 2500μm
+        auto_speed = false
+        endpoints_curvature = nothing
+        auto_curvature = false
     end
 
 Specifies rules for routing from one point to another using BSplines.
@@ -77,6 +80,9 @@ Ignores `waydirs`.
 """
 Base.@kwdef struct BSplineRouting <: RouteRule
     endpoints_speed = 2500μm
+    auto_speed = false
+    endpoints_curvature = nothing
+    auto_curvature = false
 end
 
 """
@@ -469,7 +475,10 @@ function _route!(
         vcat(waypoints, [p_end]),
         α_end,
         sty,
-        endpoints_speed=rule.endpoints_speed
+        endpoints_speed=rule.endpoints_speed,
+        auto_speed=rule.auto_speed,
+        endpoints_curvature=rule.endpoints_curvature,
+        auto_curvature=rule.auto_curvature
     )
 end
 

src/paths/segments/bspline.jl

@@ -145,7 +145,7 @@ Reconcile the interpolation `b.r` with possible changes to `b.p`, `b.t0`, `b.t1`
 
 Also updates `b.p0`, `b.p1`.
 """
-function _update_interpolation!(b::BSpline)
+function _update_interpolation!(b::BSpline{T}) where {T}
     # Use true t range for interpolations defined by points that have been scaled out of [0,1]
     tmin = b.r.ranges[1][1]
     tmax = b.r.ranges[1][end]
@@ -306,21 +306,41 @@ function curvatureradius(b::BSpline{T}, s) where {T}
 end
 
 """
-    bspline!(p::Path{T}, nextpoints, α_end, sty::Style=contstyle1(p), endpoints_speed=2500μm)
+    bspline!(p::Path{T}, nextpoints, α_end, sty::Style=contstyle1(p);
+        endpoints_speed=2500μm,
+        endpoints_curvature=nothing,
+        auto_speed=false,
+        auto_curvature=false)
 
 Add a BSpline interpolation from the current endpoint of `p` through `nextpoints`.
 
 The interpolation reaches `nextpoints[end]` making the angle `α_end` with the positive x-axis.
 The `endpoints_speed` is "how fast" the interpolation leaves and enters its endpoints. Higher
 speed means that the start and end angles are approximately α1(p) and α_end over a longer
 distance.
+
+If `auto_speed` is `true`, then `endpoints_speed` is ignored. Instead, the
+endpoint speeds are optimized to make curvature changes gradual as possible
+(minimizing the integrated square of the curvature with respect
+to arclength).
+
+If `endpoints_curvature` (dimensions of `oneunit(T)^-1`) is specified, then
+additional waypoints are placed so that the curvature at the endpoints is equal to
+`endpoints_curvature`.
+
+If `auto_curvature` is specified, then `endpoints_curvature` is ignored.
+Instead, the curvature at the end of the previous segment of the path is used, or
+zero curvature if the path was empty.
 """
 function bspline!(
     p::Path{T},
     nextpoints,
     α_end,
     sty::Style=contstyle1(p);
     endpoints_speed=2500.0 * DeviceLayout.onemicron(T),
+    endpoints_curvature=nothing,
+    auto_speed=false,
+    auto_curvature=false,
     kwargs...
 ) where {T}
     !isempty(p) &&
@@ -332,6 +352,16 @@ function bspline!(
     t0 = endpoints_speed * Point(cos(α1(p)), sin(α1(p)))
     t1 = endpoints_speed * Point(cos(α_end), sin(α_end))
     seg = BSpline(ps, t0, t1)
+    auto_curvature && (endpoints_curvature = _last_curvature(p))
+    if auto_speed
+        seg.t0 = Point(cos(α0(seg)), sin(α0(seg))) * norm(seg.p[2] - seg.p[1])
+        seg.t1 = Point(cos(α1(seg)), sin(α1(seg))) * norm(seg.p[end] - seg.p[end - 1])
+        _set_endpoints_curvature!(seg, endpoints_curvature, add_points=true)
+        _optimize_bspline!(seg; endpoints_curvature)
+    elseif !isnothing(endpoints_curvature)
+        _set_endpoints_curvature!(seg, endpoints_curvature, add_points=true)
+        _update_interpolation!(seg)
+    end
     push!(p, Node(seg, convert(ContinuousStyle, sty)))
     return nothing
 end

src/paths/segments/bspline_optimization.jl

@@ -0,0 +1,190 @@
+
+function _optimize_bspline!(b::BSpline; endpoints_curvature=nothing)
+    scale0 = Point(cos(α0(b)), sin(α0(b))) * norm(b.p[2] - b.p[1])
+    scale1 = Point(cos(α1(b)), sin(α1(b))) * norm(b.p[end] - b.p[end - 1])
+    if _symmetric_optimization(b)
+        errfunc_sym(p) = _int_dκ2(b, p[1], scale0, scale1; endpoints_curvature)
+        p = Optim.minimizer(optimize(errfunc_sym, [1.0]))
+        b.t0 = p[1] * scale0
+        b.t1 = p[1] * scale1
+    else
+        errfunc_asym(p) = _int_dκ2(b, p[1], p[2], scale0, scale1; endpoints_curvature)
+        p = Optim.minimizer(optimize(errfunc_asym, [1.0, 1.0]))
+        b.t0 = p[1] * scale0
+        b.t1 = p[2] * scale1
+    end
+    _set_endpoints_curvature!(b, endpoints_curvature)
+    return _update_interpolation!(b)
+end
+
+function _set_endpoints_curvature!(::BSpline, ::Nothing; add_points=false) end
+
+function _set_endpoints_curvature!(
+    b::BSpline{T},
+    κ0=0.0 / oneunit(T),
+    κ1=κ0;
+    add_points=false
+) where {T}
+    # Set waypoints after the start and before the end to fix curvature
+    # Usually, interpolation coefficients c are solutions to Ac = b0
+    # For tangent boundary condition, b0 = [t0, p..., t1]
+    # And A is tridiagonal (1/6, 2/3, 16) except for first and last row
+    # which are [-1/2, 0, 1/2, ...] and [..., -1/2, 0, 1/2] for tangent constraints
+    # We'll add two rows so that p_2 and p_{n-1} are unknowns
+    # Giving enough degrees of freedom to also constrain curvature
+    # Then update both A and b0 to give the following equations:
+    # 1/6 * c1 + 2/3 * c2 + 1/6 * c3 - p_2 = 0 (i.e. move p_2 to LHS)
+    # c1 - 2 * c2 + c3 = h0 (where h is the second derivative)
+    # And similarly for p_{n-1}
+    # Then solve A * c = b0 for c, so that our new p_2 is c[n-1] and p_{n-1} is c[n]
+    if add_points # Add extra waypoints rather than adjust existing ones
+        insert!(b.p, 2, b.p[1])
+        insert!(b.p, length(b.p), b.p[end])
+        # Rescale gradient BC
+        b.t0 = b.t0 * (length(b.p) - 3) / (length(b.p) - 1)
+        b.t1 = b.t1 * (length(b.p) - 3) / (length(b.p) - 1)
+    end
+    # If there are only 4 points the formula is simple to write out
+    if length(b.p) == 4 && iszero(κ0) && iszero(κ1)
+        b.p .= [
+            b.p[1],
+            5 / 6 * b.p[1] + 2 / 3 * b.t0 + 1 / 6 * b.p[end] - b.t1 / 6,
+            5 / 6 * b.p[end] - 2 / 3 * b.t1 + 1 / 6 * b.p[1] + b.t0 / 6,
+            b.p[end]
+        ]
+        return
+    end
+    # Define LHS matrix
+    n = length(b.p) + 4
+    dl = fill(1 / 6, n - 1)
+    d = fill(2 / 3, n)
+    du = fill(1 / 6, n - 1)
+    A = Array(Tridiagonal(dl, d, du))
+    A[1, 1:3] .= [-1 / 2, 0, 1 / 2] # -c1/2 + c3/2 = g0
+    A[3, n - 1] = -1 # c1/6 + 2c2/3 + c3/6 - p_2 = 0
+    A[n - 4, n] = -1 # ... -p_{n-1} = 0
+    A[n - 2, (n - 4):(n - 1)] .= [-1 / 2, 0, 1 / 2, 0] # g1, need to erase another tridiagonal too
+    A[n - 1, :] .= 0 # erase tridiagonal for κ0
+    A[n - 1, 1:3] .= [1, -2, 1] # c1 - 2c2 + c3 = h0
+    A[n, :] .= 0 # κ1
+    A[n, (n - 4):(n - 2)] .= [1, -2, 1] # κ1
+    zer = zero(Point{T})
+    # Set curvature with zero acceleration
+    h0 = Point{T}(-b.t0.y * κ0 * norm(b.t0), b.t0.x * κ0 * norm(b.t0))
+    h1 = Point{T}(-b.t1.y * κ1 * norm(b.t1), b.t1.x * κ1 * norm(b.t1))
+    # RHS
+    b0 = [b.t0, b.p[1], zer, b.p[3:(end - 2)]..., zer, b.p[end], b.t1, h0, h1]
+    # Solve
+    cx = A \ ustrip.(unit(T), getx.(b0))
+    cy = A \ ustrip.(unit(T), gety.(b0))
+    # Update points
+    b.p[2] = oneunit(T) * Point(cx[n - 1], cy[n - 1])
+    b.p[end - 1] = oneunit(T) * Point(cx[n], cy[n])
+    # The rest of c already defines the interpolation
+    # But we'll just use the usual constructor after this
+    # when _update_interpolation! is called
+    return
+end
+
+# True iff endpoints_speed should be assumed to be equal for optimization
+# I.e., tangent directions, endpoints, and waypoints have mirror or 180° symmetry
+function _symmetric_optimization(b::BSpline{T}) where {T}
+    center = (b.p0 + b.p1) / 2
+    # 180 rotation?
+    if α0(b) ≈ α1(b)
+        return isapprox(
+            reverse(RotationPi(; around_pt=center).(b.p)),
+            b.p,
+            atol=1e-3 * DeviceLayout.onenanometer(T)
+        )
+    end
+
+    # Reflection?
+    mirror_axis = Point(-(b.p1 - b.p0).y, (b.p1 - b.p0).x)
+    refl = Reflection(mirror_axis; through_pt=center)
+    return isapprox_angle(α1(b), -rotated_direction(α0(b), refl)) &&
+           isapprox(reverse(refl.(b.p)), b.p, atol=1e-3 * DeviceLayout.onenanometer(T))
+end
+
+# Integrated square of curvature derivative (scale free)
+function _int_dκ2(
+    b::BSpline{T},
+    t0,
+    t1,
+    scale0::Point{T},
+    scale1::Point{T};
+    endpoints_curvature=nothing
+) where {T}
+    t0 <= zero(t0) || t1 <= zero(t1) && return Inf
+    b.t0 = t0 * scale0
+    b.t1 = t1 * scale1
+    _set_endpoints_curvature!(b, endpoints_curvature)
+    _update_interpolation!(b)
+    return _int_dκ2(b, sqrt(norm(scale0) * norm(scale1)))
+end
+
+# Symmetric version
+function _int_dκ2(
+    b::BSpline{T},
+    t0,
+    scale0::Point{T},
+    scale1::Point{T};
+    endpoints_curvature=nothing
+) where {T}
+    t0 <= zero(t0) && return Inf
+    b.t0 = t0 * scale0
+    b.t1 = t0 * scale1
+    _set_endpoints_curvature!(b, endpoints_curvature)
+    _update_interpolation!(b)
+    return _int_dκ2(b, sqrt(norm(scale0) * norm(scale1)))
+end
+
+function _int_dκ2(b::BSpline{T}, scale) where {T}
+    G = StaticArrays.@MVector [zero(Point{T})]
+    H = StaticArrays.@MVector [zero(Point{T})]
+    J = StaticArrays.@MVector [zero(Point{T})]
+
+    return uconvert(
+        NoUnits,
+        quadgk(t -> scale^3 * (dκdt_scaled!(b, t, G, H, J))^2, 0.0, 1.0, rtol=1e-3)[1]
+    )
+end
+
+# Third derivative of Cubic BSpline (piecewise constant)
+d3_weights(::Interpolations.Cubic, _) = (-1, 3, -3, 1)
+function d3r_dt3!(J, r, t)
+    n_rescale = (length(r.itp.coefs) - 2) - 1
+    wis = Interpolations.weightedindexes(
+        (d3_weights,),
+        Interpolations.itpinfo(r)...,
+        (t * n_rescale + 1,)
+    )
+    return J[1] = Interpolations.symmatrix(
+        map(inds -> Interpolations.InterpGetindex(r)[inds...], wis)
+    )[1]
+end
+
+# Derivative of curvature with respect to pathlength
+# As a function of BSpline parameter
+function dκdt_scaled!(
+    b::BSpline{T},
+    t::Float64,
+    G::AbstractArray{Point{T}},
+    H::AbstractArray{Point{T}},
+    J::AbstractArray{Point{T}}
+) where {T}
+    Paths.Interpolations.gradient!(G, b.r, t)
+    Paths.Interpolations.hessian!(H, b.r, t)
+    d3r_dt3!(J, b.r, t)
+    g = G[1]
+    h = H[1]
+    j = J[1]
+
+    dκdt = ( # d/dt ((g.x*h.y - g.y*h.x) / ||g||^3)
+        (g.x * j.y - g.y * j.x) / norm(g)^3 +
+        -3 * (g.x * h.y - g.y * h.x) * (g.x * h.x + g.y * h.y) / norm(g)^5
+    )
+    # Return so that (dκ/dt)^2 will be normalized by speed
+    # So we can integrate over t and retain scale independence
+    return dκdt / sqrt(norm(g))
+end

src/paths/segments/offset.jl

@@ -83,6 +83,11 @@ function signed_curvature(seg::Segment, s)
     return 1 / curvatureradius(seg, s)
 end
 
+function _last_curvature(pa::Path{T}) where {T}
+    isempty(pa) && return 0.0 / oneunit(T)
+    return signed_curvature(pa[end].seg, pathlength(pa[end].seg))
+end
+
 function curvatureradius(seg::ConstantOffset, s)
     r = curvatureradius(seg.seg, s) # Ignore offset * dκds term
     return r - getoffset(seg, s)

src/utils.jl

@@ -229,7 +229,7 @@ function discretization_grid(
         )
         t = ts[i - 1]
         # Set dt based on distance from chord assuming constant curvature
-        if cc >= 1e-9 * oneunit(typeof(cc)) # Update dt if curvature is not near zero
+        if cc >= 100 * 8 * tolerance / t_scale^2 # Update dt if curvature is not near zero
             dt = uconvert(NoUnits, sqrt(8 * tolerance / cc) / t_scale)
         end
         if t + dt >= bnds[2]