diff --git a/docs/make.jl b/docs/make.jl index a6b062b5..a7525bc3 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -12,9 +12,15 @@ makedocs(modules = [DataInterpolations], linkcheck = true, format = Documenter.HTML(assets = ["assets/favicon.ico"], canonical = "https://docs.sciml.ai/DataInterpolations/stable/"), - pages = ["index.md", "Interpolation methods" => "methods.md", + pages = [ + "index.md", + "Interpolation methods" => "methods.md", "Extrapolation methods" => "extrapolation_methods.md", - "Interface" => "interface.md", "Using with Symbolics/ModelingToolkit" => "symbolics.md", - "Manual" => "manual.md", "Inverting Integrals" => "inverting_integrals.md"]) + "Interface" => "interface.md", + "Using with Symbolics/ModelingToolkit" => "symbolics.md", + "Manual" => "manual.md", + "Smooth arc length interpolation" => "arclength_interpolation.md", + "Inverting Integrals" => "inverting_integrals.md" + ]) deploydocs(repo = "github.com/SciML/DataInterpolations.jl"; push_preview = true) diff --git a/docs/src/arclength_interpolation.md b/docs/src/arclength_interpolation.md new file mode 100644 index 00000000..ffc88936 --- /dev/null +++ b/docs/src/arclength_interpolation.md @@ -0,0 +1,496 @@ +# Smooth arc length interpolation + +Arc length interpolation is interpolation between points using a curve that is parameterized by arc length. That is: the curve parameterization has unit speed everywhere, and so the parameter `t` at each point on the curve is equal to the total distance traveled from the beginning of the curve. In this context, by 'smooth' we mean that the curve is continuously differentiable. + +## Usage + +`DataInteprolations.jl` offers an arc length interpolation method that approximates an existing non arc length interpolation by circle and line segments. This can be done by providing an interpolation object (the shape interpolation): + +```@example tutorial +using DataInterpolations +using Plots +using Random + +Random.seed!(2) + +# Example from interpolation object +u = cumsum([rand(3) for _ in 1:10]) +t = 1:10 +A_shape = QuadraticSpline(u, t) +A = SmoothArcLengthInterpolation(A_shape; m = 10) + +function plot_itp(itp) + t_eval = range(itp.t[1], itp.t[end]; length = 1000) + u_eval = zeros(3, 1000) + itp(u_eval, t_eval) + + plot(eachrow(u_eval)...; label = "SmoothArcLengthInterpolation") + scatter!(eachrow(u_eval[:, 1:50:end])...; label = "Equidistant points on the curve") + scatter!(eachrow(hcat(A.shape_itp.u...))...; label = "Original data") +end + +plot_itp(A) +``` + +Here `m` determines how fine the approximation is. It is also possible to just provide the data points, optionally providing `t` and a preferred interpolation type which determines the shape of the curve. + +```@example tutorial +using LinearAlgebra + +# Example from only u +A = SmoothArcLengthInterpolation(hcat(u...)) +@show typeof(A.shape_itp) +plot_itp(A) +``` + +## Docstrings + +```@docs +SmoothArcLengthInterpolation(::AbstractMatrix{U}) where {U} +SmoothArcLengthInterpolation(::DataInterpolations.AbstractInterpolation) +SmoothArcLengthInterpolation(::AbstractMatrix, ::AbstractMatrix) +``` + +## Method derivation + +Say we have an ordered set of points $u_1, \ldots, u_n \in \mathbb{R}^N$ and we want to make a lightweight $C^1$ smooth interpolation by arc-length $\tilde{\gamma}: [0,T] \rightarrow \mathbb{R}^N$ through these points. The first part is easy, just pick your favorite established interpolation method that achieves $C^1$ smoothness. The arc-length part however turns out to be quite [nasty](https://ijpam.eu/contents/2006-31-3/10/10.pdf). Here I propose a method that is quite general and cheap to compute. + +### The 2-dimensional case + +2 is the smallest number of dimensions in which the posed problem is non-trivial. Say we use an established (non arc-length) interpolation method for our set of points to obtain the $C^1$ curve + +```math + \gamma : [0, T] \rightarrow \mathbb{R}^2 +``` + +for which + +```math + \gamma(t_i) = u_i \quad i = 1, \ldots, n, +``` + +given a suitable set of 'time' values + +```math + 0 = t_1 < t_2 < \ldots < t_n = T, +``` + +for instance + +```math + t_i = \sum_{k=1}^{i-1} \|u_{k+1} - u_k\|_2. +``` + +We now want to approximate $\gamma$ piecewise with sections that are trivially parameterizable by arc-length, namely line segments and circle segments. To do this, we fix some $m \in \mathbb{N}$ and define a refined set of time points $\left(\tilde{t}_j\right)_{j=1}^{m(n-1) + 1}$ given by + +```math + \tilde{t}_{m(k-1) + l} = t_k + \frac{l}{m + 1}(t_{k+1} - t_k), \quad k = 1 \ldots n-1, \; l = 1, \ldots m. +``` + +In these refined time points we evaluate $\gamma$ and its normalized derivative: + +```math + \tilde{u}_j = \gamma\left(\tilde{t}_j\right), \; \tilde{d}_j = \frac{\dot{\gamma}\left(\tilde{t}_j\right)}{\|\dot{\gamma}\left(\tilde{t}_j\right)\|_2}, \qquad j = 1, \ldots, m(n-1) + 1. +``` + +As a first step to create the interpolation by arc length $\tilde{\gamma}$, we make a piecewise linear curve which is tangent to $\gamma$ in $\tilde{u}_j$ for each line segment, where we denote the intersection of consecutive tangent lines by $\tilde{u}_{j, \text{int}}$: + +```math + \begin{align*} + \tilde{u}_{j, \text{int}} &=& \tilde{u}_j + \frac{\langle\tilde{u}_{j+1}-\tilde{u}_j, \tilde{d}_j\rangle - \langle\tilde{d}_j, \tilde{d}_{j+1}\rangle \langle\tilde{u}_{j+1}-\tilde{u}_j, \tilde{d}_{j+1}\rangle}{1 - \langle\tilde{d}_j, \tilde{d}_{j+1}\rangle^2}\tilde{d}_j \\ + &=& \tilde{u}_{j+1} + \frac{\langle\tilde{d}_j, \tilde{d}_{j+1}\rangle\langle\tilde{u}_{j+1}-\tilde{u}_j, \tilde{d}_j\rangle - \langle\tilde{u}_{j+1}-\tilde{u}_j, \tilde{d}_{j+1}\rangle}{1 - \langle\tilde{d}_j, \tilde{d}_{j+1}\rangle^2}\tilde{d}_{j+1}. + \end{align*} +``` + +As expected this doesn't work for $\langle\tilde{d}_j, \tilde{d}_{j+1}\rangle^2 = 1$, which means that the consecutive tangent lines are parallel. In fact, in the above equation we want the coefficient of $\tilde{d}_j$ to be positive and the coefficient of $\tilde{d}_{j+1}$ to be negative, to ensure that $\tilde{u}_{j, \text{int}}$ lies properly in between $\tilde{u}_j$ and $\tilde{u}_{j+1}$. + +```@setup tutorial +using DataInterpolations +using Plots +using Random +using LinearAlgebra + +Random.seed!(4) + +n = 4 +m = 2 +u = [rand(2) for _ in 1:n] +t = cumsum(norm.(diff(u))) +pushfirst!(t, 0) + +γ = QuadraticSpline(u, t) + +t_eval = range(first(t), last(t), length = 250) +u_eval = hcat(γ.(t_eval)...) + +t_tilde = zeros(m * (n - 1) + 1) + +for k in 1:(n-1) + t_tilde[m * (k - 1) + 1 : m * k] = range(t[k], t[k+1], length = m + 1)[1:(end - 1)] +end + +t_tilde[end] = t[end] +u_tilde = hcat(γ.(t_tilde)...) + +d_tilde = DataInterpolations.derivative.(Ref(γ), t_tilde) +normalize!.(d_tilde) +d_tilde = hcat(d_tilde...) + +n_intervals = length(t_tilde) - 1 +intersection_points = zeros(2, n_intervals) + +u_tilde_j_int_1 = zeros(2) +u_tilde_j_int_2 = zeros(2) + +for j in 1:n_intervals + d_tilde_j = view(d_tilde, :, j) + d_tilde_j_plus_1 = view(d_tilde, :, j + 1) + d_inner = dot(d_tilde_j, d_tilde_j_plus_1) + u_tilde_j = view(u_tilde, :, j) + u_tilde_j_plus_1 = view(u_tilde, :, j + 1) + Δu = u_tilde_j_plus_1 - u_tilde_j + + coef_1 = (dot(Δu, d_tilde_j) - d_inner * dot(Δu, d_tilde_j_plus_1))/(1 - d_inner^2) + coef_2 = (d_inner * dot(Δu, d_tilde_j) - dot(Δu, d_tilde_j_plus_1))/(1 - d_inner^2) + + @. u_tilde_j_int_1 = u_tilde_j + coef_1 * d_tilde_j + @. u_tilde_j_int_2 = u_tilde_j_plus_1 + coef_2 * d_tilde_j_plus_1 + @assert u_tilde_j_int_1 ≈ u_tilde_j_int_2 + + intersection_points[:, j] .= u_tilde_j_int_1 +end + +function plot_tangent_curve() + p = plot(; aspect_ratio = :equal, legend = :topleft, title = "m = $m") + + # Plot curve γ + plot!(u_eval[1,:], u_eval[2,:]; label = raw"\gamma") + + # Plot original points + u_ = hcat(u...) + scatter!(u_[1,:], u_[2,:], label = raw"$u_i$"; markersize = 6, markerstrokewidth = 0) + + # Plot refined evaluation points + scatter!(u_tilde[1,:], u_tilde[2,:]; label = raw"$\tilde{u}_j$", markerstrokewidth = 0) + + # Plot tangent curve + scatter!(intersection_points[1, :], intersection_points[2, :]; + markerstrokewidth = 0, markersize = 3, label = raw"$\tilde{u}_{j, \mathrm{int}}$") + plot!([u_tilde[1, 1], intersection_points[1, :]..., u_tilde[1, end]], + [u_tilde[2, 1], intersection_points[2, :]..., u_tilde[2, end]]; + label = "Tangent curve") + p +end +``` + +```@example tutorial +plot_tangent_curve() # hide +``` + +As a last step to obtain our curve by arc length $\tilde{\gamma}$ we want to get rid of the kinks in the tangent curve. We do this by replacing sections of the tangent curve by circle arcs. For each $\tilde{u}_{j, \text{int}}$ we compute the shortest distance to the neighboring evaluation points on $\gamma$: + +```math + \delta_j = \min\left\{ + \|\tilde{u}_j - \tilde{u}_{j, \text{int}}\|_2, + \|\tilde{u}_{j + 1} - \tilde{u}_{j, \text{int}}\|_2 + \right\}. +``` + +From this we compute 2 points that are on the tangent curve and equidistant from $\tilde{u}_{j + \frac{1}{2}}$: + +```math + \tilde{u}_{j, \text{start}} = \tilde{u}_{j, \text{int}} - \delta_j \tilde{d}_j, +``` + +```math + \tilde{u}_{j, \text{end}} = \tilde{u}_{j, \text{int}} + \delta_j \tilde{d}_{j+1}. +``` + +Note that by this definition + +```math + \tilde{u}_{j, \text{start}} = \tilde{u}_j \quad \vee \quad \tilde{u}_{j, \text{end}} = \tilde{u}_{j + 1}. +``` + +Now we can define a circle arc from $\tilde{u}_{j, \text{start}}$ to $\tilde{u}_{j, \text{end}}$ given the center + +```math + c_j = \tilde{u}_{j, \text{int}} + \delta_j\frac{\tilde{d}_{j+1} - \tilde{d}_j}{1 - \langle\tilde{d}_j,\tilde{d}_{j+1}\rangle} +``` + +and radius + +```math + R_j = \delta_j\sqrt{\frac{1 + \langle\tilde{d}_j,\tilde{d}_{j+1}\rangle}{1 - \langle\tilde{d}_j,\tilde{d}_{j+1}\rangle}}. +``` + +We obtain the circle arc + +```math + c_j + \cos\left(\frac{t}{R_j}\right)v_{j, 1} + \sin\left(\frac{t}{R_j}\right)v_{j, 2}, \quad t \in [0, \Delta t_{j, \text{arc}}], +``` + +where + +```math + v_{j, 1} = -\delta_j \frac{\tilde{d}_{j+1} - \langle\tilde{d}_j,\tilde{d}_{j+1}\rangle\tilde{d}_j}{1 - \langle\tilde{d}_j,\tilde{d}_{j+1}\rangle}, + \quad + v_{j, 2} = R_j \tilde{d}_j. +``` + +By this definition $\|v_{j, 1}\|_2 = \|v_{j, 2}\|_2 = R_j$ and $\langle v_{j, 1}, v_{j, 2}\rangle = 0$. Furthermore: + +```math + \Delta t_{j, \text{arc}} = R_j\theta_{j, \;\max}= 2R_j \arctan\left(\frac{\delta_j}{R_j}\right). +``` + +```@setup tutorial +function mark_right_angle!(corner, dir1, dir2; l = 0.05) + points = zeros(3, 2) + points[1, :] = corner + l * dir1 + points[2, :] = points[1, :] + l * dir2 + points[3, :] = points[2, :] - l * dir1 + + plot!(points[:, 1], points[:, 2]; color = :black, label = "") +end + +function plot_arc_construction(; indicate_delta = true) + p = plot(; aspect_ratio = :equal, axis = false) + + tu_j = [0.0, 0.2] + tu_j_int = [0.5, 0.6] + tu_j_plus_1 = [1.2, 0.3] + δⱼ = norm(tu_j_int - tu_j) + tu_j_start = tu_j + tu_j_end = tu_j_int + δⱼ * (tu_j_plus_1 - tu_j_int) / norm(tu_j_plus_1 - tu_j_int) + + td_j = tu_j_int - tu_j + normalize!(td_j) + td_j_plus_1 = tu_j_plus_1 - tu_j_int + normalize!(td_j_plus_1) + inner = dot(td_j, td_j_plus_1) + Δtd_j = td_j_plus_1 - td_j + + tc_j = tu_j_int + δⱼ / (1 - inner) * Δtd_j + + Rⱼ = δⱼ * sqrt((1 + inner)/(1 - inner)) + vⱼ₁ = -δⱼ * (td_j_plus_1 - inner * td_j)/(1 - inner) + vⱼ₂ = Rⱼ * td_j + Δt_j_arc = 2 * Rⱼ * atan(δⱼ, Rⱼ) + + T_ = range(0, π/2; length = 100) + x_arc = @. tc_j[1] + cos(T_) * vⱼ₁[1] + sin(T_) * vⱼ₂[1] + y_arc = @. tc_j[2] + cos(T_) * vⱼ₁[2] + sin(T_) * vⱼ₂[2] + plot!(x_arc, y_arc; label = "", color = :gray, ls = :dash) + + T = range(0, Δt_j_arc, length = 100) + X_arc = @. tc_j[1] + cos(T/Rⱼ) * vⱼ₁[1] + sin(T/Rⱼ) * vⱼ₂[1] + Y_arc = @. tc_j[2] + cos(T/Rⱼ) * vⱼ₁[2] + sin(T/Rⱼ) * vⱼ₂[2] + plot!(X_arc, Y_arc; label = "", color = :black, linewidth = 2) + + x_arc = @. tc_j[1] + cos(T/Rⱼ) * vⱼ₁[1] / 7 + sin(T/Rⱼ) * vⱼ₂[1] / 7 + y_arc = @. tc_j[2] + cos(T/Rⱼ) * vⱼ₁[2] / 7 + sin(T/Rⱼ) * vⱼ₂[2] / 7 + plot!(x_arc, y_arc; label = "", color = :black) + + z = tc_j + vⱼ₂ + + annotate!(tu_j..., "\n\n\n" * raw"$\tilde{u}_j = \tilde{u}_{j, \mathrm{start}}$") + annotate!(tu_j_int..., raw"$\tilde{u}_{j, \mathrm{int}}$" * "\n\n\n") + annotate!(tu_j_end..., raw"$\tilde{u}_{j, \mathrm{end}}$" * "\n\n\n") + annotate!(tu_j_plus_1..., "\n\n\n" * raw"$\tilde{u}_{j + 1}$") + annotate!((tu_j + td_j/5)..., raw"$\tilde{d}_j$" * " ") + annotate!((tu_j_plus_1 + td_j_plus_1/5)..., raw"$\tilde{d}_{j+1}$" * "\n\n") + annotate!(tc_j..., " " * raw"$\tilde{c}_j$") + annotate!(tc_j + vⱼ₁/2..., raw"$v_{j,1}$" * " ") + annotate!(tc_j + vⱼ₂/2..., "\n " * raw"$v_{j,2}$") + annotate!(tc_j..., raw"$\theta_{j, \max}$" * " \n\n\n\n\n") + indicate_delta && annotate!((tu_j + tu_j_int)/2..., raw"$\delta_j$" * "\n\n") + + mark_right_angle!(tu_j_start, td_j, normalize(tc_j - tu_j_start)) + mark_right_angle!(tu_j_end, td_j_plus_1, normalize(tc_j - tu_j_end)) + mark_right_angle!(tc_j, normalize(vⱼ₁), normalize(vⱼ₂)) + + # u connections + points = hcat(tu_j, tu_j_int, tu_j_end, tu_j_plus_1) + plot!(points[1, :], points[2, :]; marker = :circle, c = :black, ls = :dash, label = "") + + # line segment + plot!([tu_j_plus_1[1], tu_j_end[1]], [tu_j_plus_1[2], tu_j_end[2]], c = :black, label = "", linewidth = 2) + + # td_j and td_j_plus_1 + plot!([tu_j[1], tu_j[1] + td_j[1]/5], [tu_j[2], tu_j[2] + td_j[2]/5]; arrow=(:closed, 2.0), color = :black, label = "") + plot!([tu_j_plus_1[1], tu_j_plus_1[1] + td_j_plus_1[1]/5], [tu_j_plus_1[2], tu_j_plus_1[2] + td_j_plus_1[2]/5]; arrow=(:closed, 2.0), color = :black, label = "") + + # Circle segment radii + points = hcat(tu_j_start, tc_j, tu_j_end) + plot!(points[1, :], points[2, :]; color = :gray, label = "", ls = :dash) + + # v₂ⱼ + points = hcat(tc_j, tc_j + vⱼ₂) + plot!(points[1, :], points[2, :]; color = :gray, label = "", ls = :dash) + + # tc_j + scatter!([tc_j[1]], [tc_j[2]]; color = :gray, label = "") + ylims!(-0.65, 0.8) + return p, (; tu_j, tu_j_plus_1, tu_j_int, tu_j_start, tu_j_end, td_j, td_j_plus_1, inner, tc_j, δⱼ, Rⱼ) +end +``` + +```@example tutorial +plot_arc_construction()[1] # hide +``` + +```@setup tutorial +p = plot_tangent_curve() + +δ = [min( + norm(intersection_points[:, j] - u_tilde[:, j]), + norm(intersection_points[:, j] - u_tilde[:, j+1]) + ) + for j in 1:n_intervals +] + +u_tilde_start = zeros(2, n_intervals) +u_tilde_end = zeros(2, n_intervals) + +for j in 1:n_intervals + u_tilde_intⱼ = intersection_points[:, j] + u_tildeⱼ = u_tilde[:, j] + u_tildeⱼ₊₁ = u_tilde[:, j + 1] + d_tildeⱼ = d_tilde[:, j] + d_tildeⱼ₊₁ = d_tilde[:, j + 1] + + u_tilde_start[:, j] = u_tilde_intⱼ - δ[j] * d_tildeⱼ + u_tilde_end[:, j] = u_tilde_intⱼ + δ[j] * d_tildeⱼ₊₁ +end + +scatter!(u_tilde_start[1, :], u_tilde_start[2, :]; markersize = 3, markerstrokewidth = 0, + label = raw"$\tilde{u}_{j, start}$") +scatter!(u_tilde_end[1, :], u_tilde_end[2, :]; markersize = 3, markerstrokewidth = 0, + label = raw"$\tilde{u}_{j, end}$") + +origins = zeros(2, n_intervals) + +for j in 1:n_intervals + u_tilde_intⱼ = intersection_points[:, j] + d_tildeⱼ = d_tilde[:, j] + d_tildeⱼ₊₁ = d_tilde[:, j + 1] + inner = dot(d_tildeⱼ, d_tildeⱼ₊₁) + + origins[:, j] = u_tilde_intⱼ + δ[j] / (1 - inner) * (d_tildeⱼ₊₁ - d_tildeⱼ) + + plot!([u_tilde_start[1, j], origins[1, j], u_tilde_end[1, j]], + [u_tilde_start[2, j], origins[2, j], u_tilde_end[2, j]]; + label = (j ==1) ? "radii of circle arc" : "", ls = :dash, c = :gray) + + Rⱼ = δ[j] * sqrt((1 + inner)/(1 - inner)) + v₁ = -δ[j] * (d_tildeⱼ₊₁ - inner * d_tildeⱼ) / (1 - inner) + v₂ = Rⱼ * d_tildeⱼ + Δt = 2 * Rⱼ * atan(δ[j], Rⱼ) + T = range(0, Δt, length = 25) + x = @. origins[1, j] + cos(T/Rⱼ) * v₁[1] + sin(T/Rⱼ) * v₂[1] + y = @. origins[2, j] + cos(T/Rⱼ) * v₁[2] + sin(T/Rⱼ) * v₂[2] + plot!(x,y; c = :green, label = (j == 1) ? "circle arc" : "") +end + +scatter!(origins[1, :], origins[2, :]; label = raw"$\tilde{c}_j$", c= :gray, markerstrokewidth = 0) +``` + +```@example tutorial +p # hide +``` + +That's pretty neat, but this method does not directly generalize to higher dimensional spaces. That is because in general the intersection points $\tilde{u}_{j, \text{int}}$ of the tangent lines do not exist. + +### The higher dimensional case + +Let's try to generalize the method above. The goal is to find a point $\tilde{u}_{j + \frac{1}{2}}$ and unit direction $\tilde{d}_{j + \frac{1}{2}}$ to add to the tangent curve between $\tilde{u}_j$ and $\tilde{u}_{j+1}$ such that: + + - the tangent line intersections $\tilde{u}_{j, \text{int left}}, \tilde{u}_{j, \text{int right}}$ exist. This means that the new line is fixed by these 2 points; + - constructing $\tilde{\gamma}$ including this point gives gives an identical result to constructing $\tilde{\gamma}$ excluding this point if the tangent line intersection already existed. The latter implies that $\tilde{u}_{j + \frac{1}{2}}$ and $\tilde{d}_{j + \frac{1}{2}}$ yield a tangent line to the constructed circle arc. + +Let's assume the tangent line intersection exists, and we define + +```math + \tilde{u}_{j, \text{int left}} = \tilde{u}_{j, \text{int}} - \delta_j^* \tilde{d}_{j}, +``` + +```math + \tilde{u}_{j, \text{int right}} = \tilde{u}_{j, \text{int}} + \delta_j^* \tilde{d}_{j+1}. +``` + +It turns out that if we then let + +```math +\delta^*_j = \delta_j \frac{2 - \sqrt{2 + 2 \langle\tilde{d}_j, \tilde{d}_{j+1}\rangle}}{1 - \langle\tilde{d}_j, \tilde{d}_{j+1}\rangle}, +``` + +The line between $\tilde{u}_{, \text{int left}}$ and $ \tilde{u}_{, \text{int right}}$ touches the circle arc as constructed before. It follows that + +```math + \tilde{u}_{j + \frac{1}{2}} = \frac{1}{2}\left[\tilde{u}_{j, \text{int left}} + \tilde{u}_{j, \text{int right}}\right], + \qquad + \tilde{d}_{j + \frac{1}{2}} = \frac{\tilde{u}_{j, \text{int right}} - \tilde{u}_{j, \text{int left}}}{\|\tilde{u}_{j, \text{int right}} - \tilde{u}_{j, \text{int left}}\|_2}. +``` + +```@setup tutorial +p, vars = plot_arc_construction(; indicate_delta = false) +xlims!(0.0, 1.3) +ylims!(0.0, 0.8) +δⱼ_star = vars.δⱼ * (2 - sqrt(2 + 2 * vars.inner)) / (1 - vars.inner) +tu_j_int_left = vars.tu_j_int - δⱼ_star * vars.td_j +tu_j_int_right = vars.tu_j_int + δⱼ_star * vars.td_j_plus_1 +tu_j_plus_half = vars.tu_j_int + δⱼ_star / 2 * (vars.td_j_plus_1 - vars.td_j) +td_j_plus_half = (vars.td_j_plus_1 + vars.td_j) / sqrt(2 + 2 * vars.inner) + +scatter!([tu_j_int_left[1], tu_j_plus_half[1], tu_j_int_right[1]], + [tu_j_int_left[2], tu_j_plus_half[2], tu_j_int_right[2]]; color = :black, label = "") +annotate!(tu_j_int_left..., raw"$\tilde{u}_{j, \mathrm{int \; left}}$" * "\n\n") +annotate!(tu_j_int_right..., raw"$\tilde{u}_{j, \mathrm{int \; right}}$" * "\n\n") +annotate!(tu_j_plus_half..., "\n\n" * raw"$\tilde{u}_{j + \frac{1}{2}}$") +annotate!(tu_j_plus_half..., " " * raw"$\tilde{d}_{j + \frac{1}{2}}$" * "\n\n\n") +annotate!((vars.tu_j_int + δⱼ_star / 2 * normalize(vars.tu_j - vars.tu_j_int))..., raw"$\delta^*_j$" * "\n\n") +plot!( + [vars.tu_j_int[1] - δⱼ_star * vars.td_j[1], vars.tu_j_int[1] + δⱼ_star * vars.td_j_plus_1[1]], + [vars.tu_j_int[2] - δⱼ_star * vars.td_j[2], vars.tu_j_int[2] + δⱼ_star * vars.td_j_plus_1[2]]; + color = :black, ls = :dash, label = "") +plot!([tu_j_plus_half[1], tu_j_plus_half[1] + td_j_plus_half[1]/5], + [tu_j_plus_half[2], tu_j_plus_half[2] + td_j_plus_half[2]/5]; + arrow=(:closed, 2.0), color = :black, label = "") +``` + +```@example tutorial +p # hide +``` + +If we generalize the definition of $\tilde{u}_{j, \text{int}}$ then we can compute $\tilde{u}_{j + \frac{1}{2}}$ and $\tilde{d}_{j + \frac{1}{2}}$ as above. Something we can always compute are the points on the tangent lines which are closest together, given by: + +```math + \argmin_{s,\; t \;\in\; \mathbb{R}} \|\tilde{u}_{j+1} + s\tilde{d}_{j+1} - (\tilde{u}_j + t\tilde{d}_j)\|_2. +``` + +This yields + +```math + \tilde{u}_{j, \text{close left}} = \tilde{u}_j + \frac{\langle\tilde{u}_{j+1}-\tilde{u}_j, \tilde{d}_j\rangle - \langle\tilde{d}_j, \tilde{d}_{j+1}\rangle \langle\tilde{u}_{j+1}-\tilde{u}_j, \tilde{d}_{j+1}\rangle}{1 - \langle\tilde{d}_j, \tilde{d}_{j+1}\rangle^2}\tilde{d}_j, +``` + +```math + \tilde{u}_{j, \text{close right}} = \tilde{u}_{j+1} + \frac{\langle\tilde{d}_j, \tilde{d}_{j+1}\rangle\langle\tilde{u}_{j+1}-\tilde{u}_j, \tilde{d}_j\rangle - \langle\tilde{u}_{j+1}-\tilde{u}_j, \tilde{d}_{j+1}\rangle}{1 - \langle\tilde{d}_j, \tilde{d}_{j+1}\rangle^2}\tilde{d}_{j+1}. +``` + +This is the same as the two expressions for $\tilde{u}_{j, \text{int}}$ from before, except now these expressions aren't necessarily equal. We define $\tilde{u}_{j, \text{int}}$ as the average of these expressions: + +```math + \tilde{u}_{j, \text{int}} = \frac{\tilde{u}_{j, \text{close left}} + \tilde{u}_{j, \text{close right}}}{2}. +``` + +From this $\delta_j$ and $\delta_j^*$ follow, and + +```math + \tilde{u}_{j, \text{int left}} = \tilde{u}_{j, \text{close left}} - \delta_j^* \tilde{d}_{j}, +``` + +```math + \tilde{u}_{j, \text{int right}} = \tilde{u}_{j, \text{close right}} + \delta_j^* \tilde{d}_{j+1}. +``` diff --git a/src/DataInterpolations.jl b/src/DataInterpolations.jl index 749e4bd5..2996d84f 100644 --- a/src/DataInterpolations.jl +++ b/src/DataInterpolations.jl @@ -112,8 +112,8 @@ _output_dim(::AbstractArray{<:Any, N}) where {N} = N - 1 # each value is an arra export LinearInterpolation, QuadraticInterpolation, LagrangeInterpolation, AkimaInterpolation, ConstantInterpolation, QuadraticSpline, CubicSpline, BSplineInterpolation, BSplineApprox, CubicHermiteSpline, PCHIPInterpolation, - QuinticHermiteSpline, LinearInterpolationIntInv, ConstantInterpolationIntInv, - ExtrapolationType + QuinticHermiteSpline, SmoothArcLengthInterpolation, LinearInterpolationIntInv, + ConstantInterpolationIntInv, ExtrapolationType export output_dim # added for RegularizationSmooth, JJS 11/27/21 diff --git a/src/derivatives.jl b/src/derivatives.jl index baf0d920..4314559e 100644 --- a/src/derivatives.jl +++ b/src/derivatives.jl @@ -313,3 +313,38 @@ function _derivative( (3c₁ + (3Δt₁ + Δt₀) * c₂ + (3Δt₁^2 + Δt₀ * 2Δt₁) * c₃) out end + +function _derivative(A::SmoothArcLengthInterpolation, t::Number, iguess) + (; derivative, in_place) = A + derivative = in_place ? derivative : similar(derivative, typeof(t)) + idx = get_idx(A, t, iguess) + Δt_circ_seg = A.Δt_circle_segment[idx] + Δt_line_seg = A.Δt_line_segment[idx] + short_side_left = A.short_side_left[idx] + Δt = t - A.t[idx] + + in_circle_arc = if short_side_left + Δt < Δt_circ_seg + else + Δt > Δt_line_seg + end + + if in_circle_arc + t_circle_seg = short_side_left ? Δt : Δt - Δt_line_seg + Rⱼ = A.radius[idx] + S, C = sincos(t_circle_seg / Rⱼ) + v₁ = view(A.dir_1, :, idx) + v₂ = view(A.dir_2, :, idx) + @. derivative = (-S * v₁ + C * v₂) / Rⱼ + else + if short_side_left + d₁ = view(A.d, :, idx + 1) + @. derivative = d₁ + else + d₀ = view(A.d, :, idx) + @. derivative = d₀ + end + end + + derivative +end diff --git a/src/interpolation_caches.jl b/src/interpolation_caches.jl index aab592d1..2bc29534 100644 --- a/src/interpolation_caches.jl +++ b/src/interpolation_caches.jl @@ -1325,3 +1325,353 @@ function QuinticHermiteSpline( ddu, du, u, t, I, p, extrapolation_left, extrapolation_right, cache_parameters, linear_lookup) end + +struct SmoothArcLengthInterpolation{ + uType, tType, IType, P, D, S <: Union{AbstractInterpolation, Nothing}, T} <: + AbstractInterpolation{T} + u::uType + t::tType + d::Matrix{D} + shape_itp::S + Δt_circle_segment::Vector{P} + Δt_line_segment::Vector{P} + center::Matrix{P} + radius::Vector{P} + dir_1::Matrix{P} + dir_2::Matrix{P} + # short_side_left[i] = true means that the line segment comes after the circle segment + short_side_left::Vector{Bool} + I::IType + p::Nothing + extrapolation_left::ExtrapolationType.T + extrapolation_right::ExtrapolationType.T + iguesser::Guesser{tType} + cache_parameters::Bool + linear_lookup::Bool + out::Vector{P} + derivative::Vector{P} + in_place::Bool + function SmoothArcLengthInterpolation( + u, t, d, shape_itp, Δt_circle_segment, Δt_line_segment, + center, radius, dir_1, dir_2, short_side_left, + I, extrapolation_left, extrapolation_right, + assume_linear_t, out, derivative, in_place) + linear_lookup = seems_linear(assume_linear_t, t) + new{typeof(u), typeof(t), typeof(I), eltype(radius), + eltype(d), typeof(shape_itp), eltype(u)}( + u, t, d, shape_itp, Δt_circle_segment, Δt_line_segment, + center, radius, dir_1, dir_2, short_side_left, + I, nothing, extrapolation_left, extrapolation_right, + Guesser(t), false, linear_lookup, out, derivative, in_place + ) + end +end + +""" + SmoothArcLengthInterpolation( + u::AbstractMatrix{U}; + t::Union{AbstractVector, Nothing} = nothing, + interpolation_type::Type{<:AbstractInterpolation} = QuadraticSpline, + kwargs...) where {U} + +Interpolate in a C¹ smooth way trough the data with unit speed by approximating +an interpolation (the shape interpolation) with line segments and circle segments. + +## Arguments + + - `u`: The data to be interpolated in matrix form; (ndim, ndata). + +NOTE: With this method it is not possible to pass keyword arguments to the constructor of the shape interpolation. +If you want to do this, construct the shape interpolation yourself and use the +`SmoothArcLengthInterpolation(shape_itp::AbstractInterpolation; kwargs...)` method. + +## Keyword Arguments + + - `t`: The time points of the shape interpolation. By default given by the cumulative sum of the Euclidean + distances between the points `u`. + - `interpolation_type`: The type of the shape interpolation. Defaults to `QuadraticSpline`. Note that + for the `SmoothArcLengthInterpolation` to be C¹ smooth, the `interpolation_type` must be C¹ smooth as well. + - `m`: The number of points at which the shape interpolation is evaluated in each interval between time points. + The `SmoothArcLengthInterpolation` converges to the shape interpolation (in shape) as m → ∞. + - `extrapolation`: The extrapolation type applied left and right of the data. Possible options + are `ExtrapolationType.None` (default), `ExtrapolationType.Constant`, `ExtrapolationType.Linear` + `ExtrapolationType.Extension`, `ExtrapolationType.Periodic` and `ExtrapolationType.Reflective`. + - `extrapolation_left`: The extrapolation type applied left of the data. See `extrapolation` for + the possible options. This keyword is ignored if `extrapolation != Extrapolation.none`. + - `extrapolation_right`: The extrapolation type applied right of the data. See `extrapolation` for + the possible options. This keyword is ignored if `extrapolation != Extrapolation.none`. + - `assume_linear_t`: boolean value to specify a faster index lookup behaviour for + evenly-distributed abscissae. Alternatively, a numerical threshold may be specified + for a test based on the normalized standard deviation of the difference with respect + to the straight line (see [`looks_linear`](@ref)). Defaults to 1e-2. +""" +function SmoothArcLengthInterpolation( + u::AbstractMatrix{U}; + t::Union{AbstractVector, Nothing} = nothing, + interpolation_type::Type{<:AbstractInterpolation} = QuadraticSpline, + kwargs... +) where {U} + if isnothing(t) + # Compute default t based on point distances + N, n = size(u) + t = Vector{U}(undef, n) + t[1] = zero(U) + Δu = Vector{U}(undef, N) + for i in 2:n + @. Δu = u[:, i] - u[:, i - 1] + t[i] = t[i - 1] + norm(Δu) + end + end + shape_itp = interpolation_type(collect.(eachcol(u)), t) + SmoothArcLengthInterpolation(shape_itp; kwargs...) +end + +""" + function SmoothArcLengthInterpolation( + shape_itp::AbstractInterpolation; + m::Integer = 2, + kwargs...) + +Approximate the `shape_itp` with a C¹ unit speed interpolation using line segments and circle segments. + +## Arguments + + - `shape_itp`: The interpolation to be approximated. Note that + for the `SmoothArcLengthInterpolation` to be C¹ smooth, the `shape_itp` must be C¹ smooth as well. + +## Keyword Arguments + + - `m`: The number of points at which the shape interpolation is evaluated in each interval between time points. + The `SmoothArcLengthInterpolation` converges to the shape interpolation (in shape) as m → ∞. + - `extrapolation`: The extrapolation type applied left and right of the data. Possible options + are `ExtrapolationType.None` (default), `ExtrapolationType.Constant`, `ExtrapolationType.Linear` + `ExtrapolationType.Extension`, `ExtrapolationType.Periodic` and `ExtrapolationType.Reflective`. + - `extrapolation_left`: The extrapolation type applied left of the data. See `extrapolation` for + the possible options. This keyword is ignored if `extrapolation != Extrapolation.none`. + - `extrapolation_right`: The extrapolation type applied right of the data. See `extrapolation` for + the possible options. This keyword is ignored if `extrapolation != Extrapolation.none`. + - `assume_linear_t`: boolean value to specify a faster index lookup behaviour for + evenly-distributed abscissae. Alternatively, a numerical threshold may be specified + for a test based on the normalized standard deviation of the difference with respect + to the straight line (see [`looks_linear`](@ref)). Defaults to 1e-2. +""" +function SmoothArcLengthInterpolation( + shape_itp::AbstractInterpolation; + m::Integer = 2, + kwargs... +) + (; u, t) = shape_itp + T = promote_type(eltype(eltype(u)), eltype(t)) + + # Resp. the output dimensionality and the number of data points in the original interpolation + N = length(first(u)) + n = length(u) + + # Number of points defining the tangent curve of shape_itp + n_tilde = m * (n - 1) + 1 + + # The evaluations of shape_itp + u_tilde = Matrix{T}(undef, N, n_tilde) + d_tilde = Matrix{T}(undef, N, n_tilde) + + j = 1 + + for i in 1:(n - 1) + for t_eval in range(t[i], t[i + 1], length = m + 1)[1:(end - 1)] + u_tilde[:, j] .= shape_itp(t_eval) + d_tilde[:, j] .= derivative(shape_itp, t_eval) + normalize!(view(d_tilde, :, j)) + j += 1 + end + end + + u_tilde[:, end] .= shape_itp(last(t)) + d_tilde[:, end] .= derivative(shape_itp, last(t)) + normalize!(view(d_tilde, :, n_tilde)) + + return SmoothArcLengthInterpolation(u_tilde, d_tilde; shape_itp, kwargs...) +end + +""" + function SmoothArcLengthInterpolation( + u::AbstractMatrix, + d::AbstractMatrix + [, make_intersections::Val{<:Bool}]; + shape_itp::Union{AbstractInterpolation, Nothing} = nothing, + extrapolation::ExtrapolationType.T = ExtrapolationType.None, + extrapolation_left::ExtrapolationType.T = ExtrapolationType.None, + extrapolation_right::ExtrapolationType.T = ExtrapolationType.None, + cache_parameters::Bool = false, + assume_linear_t = 1e-2, + in_place::Bool = true) + +Make a C¹ smooth unit speed interpolation through the given data with the given tangents using line +segments and circle segments. + +## Arguments + + - `u`: The data to be interpolated in matrix form; (ndim, ndata). + - `d`: The tangents to the curve in the points `u`. + - `make_intersections`: Whether additional (point, tangent) pairs have to be added in between the provided + data to ensure that the consecutive (tangent) lines intersect. Defaults to `Val(true)`. + +## Keyword Arguments + + - `shape_itp`: The interpolation that is being approximated, if one exists. Note that this + interpolation is not being used; it is just passed along to keep track of where the shape + of the `SmoothArcLengthInterpolation` originated. + - `extrapolation`: The extrapolation type applied left and right of the data. Possible options + are `ExtrapolationType.None` (default), `ExtrapolationType.Constant`, `ExtrapolationType.Linear` + `ExtrapolationType.Extension`, `ExtrapolationType.Periodic` and `ExtrapolationType.Reflective`. + - `extrapolation_left`: The extrapolation type applied left of the data. See `extrapolation` for + the possible options. This keyword is ignored if `extrapolation != Extrapolation.none`. + - `extrapolation_right`: The extrapolation type applied right of the data. See `extrapolation` for + the possible options. This keyword is ignored if `extrapolation != Extrapolation.none`. + - `assume_linear_t`: boolean value to specify a faster index lookup behaviour for + evenly-distributed abscissae. Alternatively, a numerical threshold may be specified + for a test based on the normalized standard deviation of the difference with respect + to the straight line (see [`looks_linear`](@ref)). Defaults to 1e-2. +""" +function SmoothArcLengthInterpolation( + u::AbstractMatrix, + d::AbstractMatrix; + kwargs...) + SmoothArcLengthInterpolation(u, d, Val{true}(); kwargs...) +end + +function SmoothArcLengthInterpolation( + u::AbstractMatrix, + d::AbstractMatrix, + make_intersections::Val{true}; + kwargs...) + N, n = size(u) + + # Number of points in the augmented tangent curve + n_hat = 2 * n - 1 + + # The data defining the augmented tangent curve + T = promote_type(eltype(eltype(u)), eltype(d)) + u_hat = Matrix{T}(undef, N, n_hat) + d_hat = Matrix{T}(undef, N, n_hat) + + k = 1 + Δu = Vector{T}(undef, N) + uⱼ_close_left = Vector{T}(undef, N) + uⱼ_close_right = Vector{T}(undef, N) + uⱼ_int = Vector{T}(undef, N) + uⱼ_int_left = Vector{T}(undef, N) + uⱼ_int_right = Vector{T}(undef, N) + + for j in 1:(n - 1) + u_hat[:, k] .= u[:, j] + d_hat[:, k] .= d[:, j] + + uⱼ, uⱼ₊₁, dⱼ, dⱼ₊₁, d_inner = smooth_arc_length_params_1!(Δu, u, d, j) + + inner_1 = dot(Δu, dⱼ) + inner_2 = dot(Δu, dⱼ₊₁) + denom = 1 - d_inner^2 + dⱼ_coef = (inner_1 - d_inner * inner_2) / denom + dⱼ₊₁_coef = (d_inner * inner_1 - inner_2) / denom + + if !((dⱼ_coef >= 0) && (dⱼ₊₁_coef <= 0)) + error("Some consecutive tangent lines do not converge, consider increasing m.") + end + + @. uⱼ_close_left = uⱼ + dⱼ_coef * dⱼ + @. uⱼ_close_right = uⱼ₊₁ + + dⱼ₊₁_coef * dⱼ₊₁ + @. uⱼ_int = (uⱼ_close_left + uⱼ_close_right) / 2 + + # compute δ_star + δⱼ, _, _ = smooth_arc_length_params_2(uⱼ_int, uⱼ, uⱼ₊₁) + δⱼ_star = δⱼ * (2 - sqrt(2 + 2 * d_inner)) / (1 - d_inner) + + # Compute the points whose connecting line defines the tangent curve augmenting point + @. uⱼ_int_left = uⱼ_close_left - δⱼ_star * dⱼ + @. uⱼ_int_right = uⱼ_close_right + δⱼ_star * dⱼ₊₁ + + # Compute tangent curve augmenting point + uⱼ_plus_half = view(u_hat, :, k + 1) + dⱼ_plus_half = view(d_hat, :, k + 1) + + @. uⱼ_plus_half = (uⱼ_int_left + uⱼ_int_right) / 2 + @. dⱼ_plus_half = uⱼ_int_right - uⱼ_int_left + normalize!(dⱼ_plus_half) + + k += 2 + end + + u_hat[:, end] .= u[:, end] + d_hat[:, end] .= d[:, end] + + return SmoothArcLengthInterpolation(u_hat, d_hat, Val{false}(); kwargs...) +end + +function SmoothArcLengthInterpolation( + u::AbstractMatrix, + d::AbstractMatrix, + ::Val{false}; + shape_itp::Union{AbstractInterpolation, Nothing} = nothing, + extrapolation::ExtrapolationType.T = ExtrapolationType.None, + extrapolation_left::ExtrapolationType.T = ExtrapolationType.None, + extrapolation_right::ExtrapolationType.T = ExtrapolationType.None, + cache_parameters::Bool = false, + assume_linear_t = 1e-2, + in_place::Bool = true) + N = size(u, 1) + n_circle_arcs = size(u, 2) - 1 + + P = promote_type(eltype(u), eltype(d)) + t = zeros(P, n_circle_arcs + 1) + Δt_circle_segment = zeros(P, n_circle_arcs) + Δt_line_segment = zeros(P, n_circle_arcs) + center = Matrix{P}(undef, N, n_circle_arcs) + radius = Vector{P}(undef, n_circle_arcs) + dir_1 = Matrix{P}(undef, N, n_circle_arcs) + dir_2 = Matrix{P}(undef, N, n_circle_arcs) + short_side_left = zeros(Bool, n_circle_arcs) + + # Intermediate results + Δu = Vector{P}(undef, N) + u_int = Vector{P}(undef, N) + + # Compute circle segments and line segments + for j in 1:n_circle_arcs + uⱼ, uⱼ₊₁, dⱼ, dⱼ₊₁, d_inner = smooth_arc_length_params_1!(Δu, u, d, j) + + dⱼ_coef = (dot(Δu, dⱼ) - d_inner * dot(Δu, dⱼ₊₁)) / (1 - d_inner^2) + @. u_int = uⱼ + dⱼ_coef * dⱼ + + δⱼ, short_side_left_, Δt_line_seg = smooth_arc_length_params_2(u_int, uⱼ, uⱼ₊₁) + short_side_left[j] = short_side_left_ + + Rⱼ = δⱼ * sqrt((1 + d_inner) / (1 - d_inner)) + radius[j] = Rⱼ + cⱼ = view(center, :, j) + v₁ = view(dir_1, :, j) + v₂ = view(dir_2, :, j) + + @. cⱼ = u_int + δⱼ * (dⱼ₊₁ - dⱼ) / (1 - d_inner) + @. v₁ = -δⱼ * (dⱼ₊₁ - d_inner * dⱼ) / (1 - d_inner) + @. v₂ = Rⱼ * dⱼ + + Δt_circle_seg = 2Rⱼ * atan(δⱼ, Rⱼ) + Δt_circle_segment[j] = Δt_circle_seg + Δt_line_segment[j] = Δt_line_seg + + t[j + 1] = t[j] + Δt_circle_seg + Δt_line_seg + end + + extrapolation_left, extrapolation_right = munge_extrapolation( + extrapolation, extrapolation_left, extrapolation_right) + linear_lookup = seems_linear(assume_linear_t, t) + + out = Vector{P}(undef, N) + derivative = Vector{P}(undef, N) + + return SmoothArcLengthInterpolation( + u, t, d, shape_itp, Δt_circle_segment, Δt_line_segment, + center, radius, dir_1, dir_2, short_side_left, + nothing, extrapolation_left, extrapolation_right, linear_lookup, out, derivative, in_place) +end diff --git a/src/interpolation_methods.jl b/src/interpolation_methods.jl index 73e240d1..5d0c1e3e 100644 --- a/src/interpolation_methods.jl +++ b/src/interpolation_methods.jl @@ -336,3 +336,42 @@ function _interpolate( out += Δt₀^3 * (c₁ + Δt₁ * (c₂ + c₃ * Δt₁)) out end + +function _interpolate(A::SmoothArcLengthInterpolation, t::Number, iguess) + (; out, in_place) = A + out = in_place ? out : similar(out, typeof(t)) + idx = get_idx(A, t, iguess) + Δt_circ_seg = A.Δt_circle_segment[idx] + Δt_line_seg = A.Δt_line_segment[idx] + short_side_left = A.short_side_left[idx] + Δt = t - A.t[idx] + + in_circle_arc = if short_side_left + Δt < Δt_circ_seg + else + Δt > Δt_line_seg + end + + if in_circle_arc + t_circle_seg = short_side_left ? Δt : Δt - Δt_line_seg + Rⱼ = A.radius[idx] + S, C = sincos(t_circle_seg / Rⱼ) + c = view(A.center, :, idx) + v₁ = view(A.dir_1, :, idx) + v₂ = view(A.dir_2, :, idx) + @. out = c + C * v₁ + S * v₂ + else + if short_side_left + u₁ = view(A.u, :, idx + 1) + d₁ = view(A.d, :, idx + 1) + t_line_seg = A.t[idx + 1] - t + @. out = u₁ - t_line_seg * d₁ + else + u₀ = view(A.u, :, idx) + d₀ = view(A.d, :, idx) + @. out = u₀ + Δt * d₀ + end + end + + out +end diff --git a/src/interpolation_utils.jl b/src/interpolation_utils.jl index ce4f6624..bf06a19e 100644 --- a/src/interpolation_utils.jl +++ b/src/interpolation_utils.jl @@ -313,3 +313,32 @@ end typed_nan(::AbstractArray{T}) where {T <: AbstractFloat} = T(NaN) typed_nan(::AbstractArray{T}) where {T <: Integer} = zero(T) + +# Should be replaceable by LinearAlgebra function soon: https://github.com/JuliaLang/LinearAlgebra.jl/pull/1234 +function euclidean(x::AbstractArray, y::AbstractArray) + sqrt(mapreduce((xi, yi) -> abs2(yi - xi), +, x, y)) +end + +function smooth_arc_length_params_1!(Δu, u, d, j) + uⱼ = view(u, :, j) + uⱼ₊₁ = view(u, :, j + 1) + dⱼ = view(d, :, j) + dⱼ₊₁ = view(d, :, j + 1) + @. Δu = uⱼ₊₁ - uⱼ + d_inner = dot(dⱼ, dⱼ₊₁) + return uⱼ, uⱼ₊₁, dⱼ, dⱼ₊₁, d_inner +end + +function smooth_arc_length_params_2(u_int, uⱼ, uⱼ₊₁) + dist₁ = euclidean(u_int, uⱼ) + dist₂ = euclidean(u_int, uⱼ₊₁) + Δt_line_seg = abs(dist₂ - dist₁) + short_side_left = false + δⱼ = if dist₁ < dist₂ + short_side_left = true + dist₁ + else + dist₂ + end + return δⱼ, short_side_left, Δt_line_seg +end diff --git a/test/derivative_tests.jl b/test/derivative_tests.jl index 376b3310..433e600a 100644 --- a/test/derivative_tests.jl +++ b/test/derivative_tests.jl @@ -7,6 +7,7 @@ using StableRNGs using RegularizationTools using Optim using ForwardDiff +using LinearAlgebra function test_derivatives(method; args = [], kwargs = [], name::String) kwargs_extrapolation = (method == Curvefit) ? @@ -28,10 +29,11 @@ function test_derivatives(method; args = [], kwargs = [], name::String) @test isapprox(cdiff2, adiff2, atol = 1e-8) end + func isa SmoothArcLengthInterpolation && return + # Interpolation time points for _t in t[2:(end - 1)] - if func isa BSplineInterpolation || func isa BSplineApprox || - func isa CubicHermiteSpline + if func isa Union{BSplineInterpolation, BSplineApprox, CubicHermiteSpline} fdiff = forward_fdm(5, 1; geom = true)(func, _t) fdiff2 = forward_fdm(5, 1; geom = true)(t -> derivative(func, t), _t) else @@ -53,7 +55,8 @@ function test_derivatives(method; args = [], kwargs = [], name::String) fdiff = forward_fdm(5, 1; geom = true)(func, t[1]) adiff = derivative(func, t[1]) @test isapprox(fdiff, adiff, atol = 1e-8) - if !(func isa BSplineInterpolation || func isa BSplineApprox) + if !(func isa + Union{BSplineInterpolation, BSplineApprox, SmoothArcLengthInterpolation}) fdiff2 = forward_fdm(5, 1; geom = true)(t -> derivative(func, t), t[1]) adiff2 = derivative(func, t[1], 2) @test isapprox(fdiff2, adiff2, atol = 1e-8) @@ -63,7 +66,8 @@ function test_derivatives(method; args = [], kwargs = [], name::String) fdiff = backward_fdm(5, 1; geom = true)(func, t[end]) adiff = derivative(func, t[end]) @test isapprox(fdiff, adiff, atol = 1e-8) - if !(func isa BSplineInterpolation || func isa BSplineApprox) + if !(func isa + Union{BSplineInterpolation, BSplineApprox, SmoothArcLengthInterpolation}) fdiff2 = backward_fdm(5, 1; geom = true)(t -> derivative(func, t), t[end]) adiff2 = derivative(func, t[end], 2) @test isapprox(fdiff2, adiff2, atol = 1e-8) @@ -262,6 +266,18 @@ end @test derivative(A, 300.0)≈0.0331361 rtol=1e-5 end +@testset "Smooth Arc Length Interpolation" begin + u = [0.3 -1.5 3.1; -0.2 0.2 -1.5; 10.4 -37.2 -5.8] + test_derivatives( + SmoothArcLengthInterpolation, args = [u], kwargs = Pair[ + :m => 5, :in_place => false], + name = "Smooth Arc Length Interpolation") + A = SmoothArcLengthInterpolation(u'; m = 25, in_place = false) + @test all(t -> norm(derivative(A, t)) ≈ 1, range(0, A.t[end]; length = 100)) + @test all( + t_ -> derivative(A, prevfloat(t_)) ≈ derivative(A, nextfloat(t_)), A.t[2:(end - 1)]) +end + @testset "RegularizationSmooth" begin npts = 50 xmin = 0.0 diff --git a/test/interpolation_tests.jl b/test/interpolation_tests.jl index 16e33562..43eaf659 100644 --- a/test/interpolation_tests.jl +++ b/test/interpolation_tests.jl @@ -4,6 +4,7 @@ using StableRNGs using Optim, ForwardDiff using BenchmarkTools using Unitful +using LinearAlgebra function test_interpolation_type(T) @test T <: DataInterpolations.AbstractInterpolation @@ -910,6 +911,29 @@ end @test @inferred(output_dim(A)) == 0 end +@testset "Smooth Arc Length Interpolation" begin + # Directly from data (assuming line intersections exist) + u = [0.0 0.0 0.0; 1.0 1.0 1.0] + d = [0.0 0.0 1.0; inv(sqrt(2)) inv(sqrt(2)) 0.0] + A = SmoothArcLengthInterpolation(u', d', Val{false}()) + @test isnothing(A.shape_itp) + @test only(A.Δt_circle_segment) ≈ π / 2 + @test only(A.Δt_line_segment) ≈ sqrt(2) - 1 + @test A.center ≈ [inv(sqrt(2)), inv(sqrt(2)), 0] + @test only(A.radius) ≈ 1 + @test A.dir_1 ≈ [-inv(sqrt(2)), -inv(sqrt(2)), 0] + @test A.dir_2 ≈ [0.0, 0.0, 1.0] + @test only(A.short_side_left) + + # From interpolation object + u = [0.3 -1.5 3.1; -0.2 0.2 -1.5; 0.0 0.0 0.0] + A = SmoothArcLengthInterpolation(u'; m = 25) + L = sum(norm.(diff(A.shape_itp(range(0, A.shape_itp.t[end]; length = 1_000_000))))) + @test A.t[end]≈L rtol=1e-4 + @test all( + t_ -> A(prevfloat(t_)) ≈ A(nextfloat(t_)), A.t[2:(end - 1)]) +end + @testset "Curvefit" begin # Curvefit Interpolation rng = StableRNG(12345)