diff --git a/src/interpolation_caches.jl b/src/interpolation_caches.jl index ca8e7b36..29f7224a 100644 --- a/src/interpolation_caches.jl +++ b/src/interpolation_caches.jl @@ -597,7 +597,7 @@ struct BSplineInterpolation{uType, tType, pType, kType, cType, scType, T, N} <: end function BSplineInterpolation( - u, t, d, pVecType, knotVecType; extrapolate = false, assume_linear_t = 1e-2) + u::AbstractVector, t, d, pVecType, knotVecType; extrapolate = false, assume_linear_t = 1e-2) u, t = munge_data(u, t) n = length(t) n < d + 1 && error("BSplineInterpolation needs at least d + 1, i.e. $(d+1) points.") @@ -665,6 +665,79 @@ function BSplineInterpolation( u, t, d, p, k, c, sc, pVecType, knotVecType, extrapolate, assume_linear_t) end +function BSplineInterpolation( + u::AbstractArray{T, N}, t, d, pVecType, knotVecType; extrapolate = false, + assume_linear_t = 1e-2) where {T, N} + u, t = munge_data(u, t) + n = length(t) + n < d + 1 && error("BSplineInterpolation needs at least d + 1, i.e. $(d+1) points.") + s = zero(eltype(u)) + p = zero(t) + k = zeros(eltype(t), n + d + 1) + l = zeros(eltype(u), n - 1) + p[1] = zero(eltype(t)) + p[end] = one(eltype(t)) + + ax_u = axes(u)[1:(end - 1)] + + for i in 2:n + s += √((t[i] - t[i - 1])^2 + sum((u[ax_u..., i] - u[ax_u..., i - 1]) .^ 2)) + l[i - 1] = s + end + if pVecType == :Uniform + for i in 2:(n - 1) + p[i] = p[1] + (i - 1) * (p[end] - p[1]) / (n - 1) + end + elseif pVecType == :ArcLen + for i in 2:(n - 1) + p[i] = p[1] + l[i - 1] / s * (p[end] - p[1]) + end + end + + lidx = 1 + ridx = length(k) + while lidx <= (d + 1) && ridx >= (length(k) - d) + k[lidx] = p[1] + k[ridx] = p[end] + lidx += 1 + ridx -= 1 + end + + ps = zeros(eltype(t), n - 2) + s = zero(eltype(t)) + for i in 2:(n - 1) + s += p[i] + ps[i - 1] = s + end + + if knotVecType == :Uniform + # uniformly spaced knot vector + # this method is not recommended because, if it is used with the chord length method for global interpolation, + # the system of linear equations would be singular. + for i in (d + 2):n + k[i] = k[1] + (i - d - 1) // (n - d) * (k[end] - k[1]) + end + elseif knotVecType == :Average + # average spaced knot vector + idx = 1 + if d + 2 <= n + k[d + 2] = 1 // d * ps[d] + end + for i in (d + 3):n + k[i] = 1 // d * (ps[idx + d] - ps[idx]) + idx += 1 + end + end + # control points + sc = zeros(eltype(t), n, n) + spline_coefficients!(sc, d, k, p) + c = (sc \ reshape(u, prod(size(u)[1:(end - 1)]), :)')' + c = reshape(c, size(u)...) + sc = zeros(eltype(t), n) + BSplineInterpolation( + u, t, d, p, k, c, sc, pVecType, knotVecType, extrapolate, assume_linear_t) +end + """ BSplineApprox(u, t, d, h, pVecType, knotVecType; extrapolate = false) diff --git a/src/interpolation_methods.jl b/src/interpolation_methods.jl index 5223d416..009f2ea3 100644 --- a/src/interpolation_methods.jl +++ b/src/interpolation_methods.jl @@ -197,6 +197,25 @@ function _interpolate(A::BSplineInterpolation{<:AbstractVector{<:Number}}, ucum end +function _interpolate(A::BSplineInterpolation{<:AbstractArray{T, N}}, + t::Number, + iguess) where {T <: Number, N} + ax_u = axes(A.u)[1:(end - 1)] + t < A.t[1] && return A.u[ax_u..., 1] + t > A.t[end] && return A.u[ax_u..., end] + # change t into param [0 1] + idx = get_idx(A, t, iguess) + t = A.p[idx] + (t - A.t[idx]) / (A.t[idx + 1] - A.t[idx]) * (A.p[idx + 1] - A.p[idx]) + n = length(A.t) + sc = t isa ForwardDiff.Dual ? zeros(eltype(t), n) : A.sc + nonzero_coefficient_idxs = spline_coefficients!(sc, A.d, A.k, t) + ucum = zeros(eltype(A.u), size(A.u)[1:(end - 1)]...) + for i in nonzero_coefficient_idxs + ucum = ucum + (sc[i] * A.c[ax_u..., i]) + end + ucum +end + # BSpline Curve Approx function _interpolate(A::BSplineApprox{<:AbstractVector{<:Number}}, t::Number, iguess) t < A.t[1] && return A.u[1]