diff --git a/src/derivatives.jl b/src/derivatives.jl index 86640360..cc57f066 100644 --- a/src/derivatives.jl +++ b/src/derivatives.jl @@ -62,9 +62,10 @@ function _derivative(A::LagrangeInterpolation{<:AbstractVector}, t::Number) der end -function _derivative(A::LagrangeInterpolation{<:AbstractMatrix}, t::Number) +function _derivative(A::LagrangeInterpolation{<:AbstractArray}, t::Number) ((t < A.t[1] || t > A.t[end]) && !A.extrapolate) && throw(ExtrapolationError()) - der = zero(A.u[:, 1]) + ax = axes(A.u)[1:(end - 1)] + der = zero(A.u[ax..., 1]) for j in eachindex(A.t) tmp = zero(A.t[1]) if isnan(A.bcache[j]) @@ -91,7 +92,7 @@ function _derivative(A::LagrangeInterpolation{<:AbstractMatrix}, t::Number) tmp += k end end - der += A.u[:, j] * tmp + der += A.u[ax..., j] * tmp end der end @@ -99,7 +100,7 @@ end function _derivative(A::LagrangeInterpolation{<:AbstractVector}, t::Number, idx) _derivative(A, t) end -function _derivative(A::LagrangeInterpolation{<:AbstractMatrix}, t::Number, idx) +function _derivative(A::LagrangeInterpolation{<:AbstractArray}, t::Number, idx) _derivative(A, t) end @@ -110,6 +111,14 @@ function _derivative(A::AkimaInterpolation{<:AbstractVector}, t::Number, iguess) @evalpoly wj A.b[idx] 2A.c[j] 3A.d[j] end +function _derivative(A::AkimaInterpolation{<:AbstractArray}, t::Number, iguess) + idx = get_idx(A, t, iguess; idx_shift = -1, side = :first) + j = min(idx, length(A.c)) # for smooth derivative at A.t[end] + wj = t - A.t[idx] + ax = axes(A.u)[1:(end - 1)] + @. @evalpoly wj A.b[ax..., idx] 2A.c[ax..., j] 3A.d[ax..., j] +end + function _derivative(A::ConstantInterpolation, t::Number, iguess) return zero(first(A.u)) end @@ -119,13 +128,15 @@ function _derivative(A::ConstantInterpolation{<:AbstractVector}, t::Number, igue return isempty(searchsorted(A.t, t)) ? zero(A.u[1]) : eltype(A.u)(NaN) end -function _derivative(A::ConstantInterpolation{<:AbstractMatrix}, t::Number, iguess) +function _derivative(A::ConstantInterpolation{<:AbstractArray}, t::Number, iguess) ((t < A.t[1] || t > A.t[end]) && !A.extrapolate) && throw(ExtrapolationError()) - return isempty(searchsorted(A.t, t)) ? zero(A.u[:, 1]) : eltype(A.u)(NaN) .* A.u[:, 1] + ax = axes(A.u)[1:(end - 1)] + return isempty(searchsorted(A.t, t)) ? zero(A.u[ax..., 1]) : + eltype(A.u)(NaN) .* A.u[ax..., 1] end # QuadraticSpline Interpolation -function _derivative(A::QuadraticSpline{<:AbstractVector}, t::Number, iguess) +function _derivative(A::QuadraticSpline, t::Number, iguess) idx = get_idx(A, t, iguess; lb = 2, ub_shift = 0, side = :first) σ = get_parameters(A, idx - 1) A.z[idx - 1] + 2σ * (t - A.t[idx - 1]) @@ -143,6 +154,18 @@ function _derivative(A::CubicSpline{<:AbstractVector}, t::Number, iguess) dI + dC + dD end +function _derivative(A::CubicSpline{<:AbstractArray}, t::Number, iguess) + idx = get_idx(A, t, iguess) + Δt₁ = t - A.t[idx] + Δt₂ = A.t[idx + 1] - t + ax = axes(A.u)[1:(end - 1)] + dI = (-A.z[ax..., idx] * Δt₂^2 + A.z[ax..., idx + 1] * Δt₁^2) / (2A.h[idx + 1]) + c₁, c₂ = get_parameters(A, idx) + dC = c₁ + dD = -c₂ + dI + dC + dD +end + function _derivative(A::BSplineInterpolation{<:AbstractVector{<:Number}}, t::Number, iguess) # change t into param [0 1] t < A.t[1] && return zero(A.u[1]) @@ -197,6 +220,18 @@ function _derivative( out end +function _derivative( + A::CubicHermiteSpline{<:AbstractArray}, t::Number, iguess) + idx = get_idx(A, t, iguess) + Δt₀ = t - A.t[idx] + Δt₁ = t - A.t[idx + 1] + ax = axes(A.u)[1:(end - 1)] + out = A.du[ax..., idx] + c₁, c₂ = get_parameters(A, idx) + out .+= Δt₀ .* (Δt₀ .* c₂ .+ 2(c₁ .+ Δt₁ .* c₂)) + out +end + # Quintic Hermite Spline function _derivative( A::QuinticHermiteSpline{<:AbstractVector{<:Number}}, t::Number, iguess) @@ -209,3 +244,16 @@ function _derivative( (3c₁ + (3Δt₁ + Δt₀) * c₂ + (3Δt₁^2 + Δt₀ * 2Δt₁) * c₃) out end + +function _derivative( + A::QuinticHermiteSpline{<:AbstractArray}, t::Number, iguess) + idx = get_idx(A, t, iguess) + Δt₀ = t - A.t[idx] + Δt₁ = t - A.t[idx + 1] + ax = axes(A.u)[1:(end - 1)] + out = A.du[ax..., idx] + A.ddu[ax..., idx] * Δt₀ + c₁, c₂, c₃ = get_parameters(A, idx) + out .+= Δt₀^2 .* + (3c₁ .+ (3Δt₁ .+ Δt₀) .* c₂ + (3Δt₁^2 .+ Δt₀ .* 2Δt₁) .* c₃) + out +end diff --git a/src/integrals.jl b/src/integrals.jl index 3ae893f7..8c15d094 100644 --- a/src/integrals.jl +++ b/src/integrals.jl @@ -61,7 +61,6 @@ function _integral(A::QuadraticInterpolation{<:AbstractVector{<:Number}}, t₀ = A.t[idx] t₁ = A.t[idx + 1] t₂ = A.t[idx + 2] - t_sq = (t^2) / 3 l₀, l₁, l₂ = get_parameters(A, idx) Iu₀ = l₀ * t * (t_sq - t * (t₁ + t₂) / 2 + t₁ * t₂) diff --git a/src/interpolation_caches.jl b/src/interpolation_caches.jl index ca8e7b36..cfca16cf 100644 --- a/src/interpolation_caches.jl +++ b/src/interpolation_caches.jl @@ -189,8 +189,7 @@ struct AkimaInterpolation{uType, tType, IType, bType, cType, dType, T, N} <: u, t, I, b, c, d, extrapolate, cache_parameters, assume_linear_t) linear_lookup = seems_linear(assume_linear_t, t) N = get_output_dim(u) - new{typeof(u), typeof(t), typeof(I), typeof(b), typeof(c), - typeof(d), eltype(u), N}(u, + new{typeof(u), typeof(t), typeof(I), typeof(b), typeof(c), typeof(d), eltype(u), N}(u, t, I, b, @@ -205,7 +204,9 @@ struct AkimaInterpolation{uType, tType, IType, bType, cType, dType, T, N} <: end function AkimaInterpolation( - u, t; extrapolate = false, cache_parameters = false, assume_linear_t = 1e-2) + u::uType, t; extrapolate = false, cache_parameters = false, + assume_linear_t = 1e-2) where {uType <: + AbstractVector{<:Number}} u, t = munge_data(u, t) linear_lookup = seems_linear(assume_linear_t, t) n = length(t) @@ -216,7 +217,6 @@ function AkimaInterpolation( m[1] = 2m[2] - m[3] m[end - 1] = 2m[end - 2] - m[end - 3] m[end] = 2m[end - 1] - m[end - 2] - b = 0.5 .* (m[4:end] .+ m[1:(end - 3)]) dm = abs.(diff(m)) f1 = dm[3:(n + 2)] @@ -227,7 +227,45 @@ function AkimaInterpolation( f2[ind] .* m[ind .+ 2]) ./ f12[ind] c = (3.0 .* m[3:(end - 2)] .- 2.0 .* b[1:(end - 1)] .- b[2:end]) ./ dt d = (b[1:(end - 1)] .+ b[2:end] .- 2.0 .* m[3:(end - 2)]) ./ dt .^ 2 + A = AkimaInterpolation( + u, t, nothing, b, c, d, extrapolate, cache_parameters, linear_lookup) + I = cumulative_integral(A, cache_parameters) + AkimaInterpolation(u, t, I, b, c, d, extrapolate, cache_parameters, linear_lookup) +end +function AkimaInterpolation( + u::uType, t; extrapolate = false, cache_parameters = false, + assume_linear_t = 1e-2) where {uType <: + AbstractArray} + n = length(t) + dt = diff(t) + ax = axes(u)[1:(end - 1)] + su = size(u) + m = zeros(eltype(u), su[1:(end - 1)]..., n + 3) + m[ax..., 3:(end - 2)] .= mapslices( + x -> x ./ dt, diff(u, dims = length(su)); dims = length(su)) + m[ax..., 2] .= 2m[ax..., 3] .- m[ax..., 4] + m[ax..., 1] .= 2m[ax..., 2] .- m[3] + m[ax..., end - 1] .= 2m[ax..., end - 2] - m[ax..., end - 3] + m[ax..., end] .= 2m[ax..., end - 1] .- m[ax..., end - 2] + b = 0.5 .* (m[ax..., 4:end] .+ m[ax..., 1:(end - 3)]) + dm = abs.(diff(m, dims = length(su))) + f1 = dm[ax..., 3:(n + 2)] + f2 = dm[ax..., 1:n] + f12 = f1 .+ f2 + ind = findall(f12 .> 1e-9 * maximum(f12)) + indi = map(i -> i.I, ind) + b[ind] .= (f1[ind] .* + m[CartesianIndex.(map(i -> (i[1:(end - 1)]..., i[end] + 1), indi))] .+ + f2[ind] .* + m[CartesianIndex.(map(i -> (i[1:(end - 1)]..., i[end] + 2), indi))]) ./ + f12[ind] + c = mapslices(x -> x ./ dt, + (3.0 .* m[ax..., 3:(end - 2)] .- 2.0 .* b[ax..., 1:(end - 1)] .- b[ax..., 2:end]); + dims = length(su)) + d = mapslices(x -> x ./ dt .^ 2, + (b[ax..., 1:(end - 1)] .+ b[ax..., 2:end] .- 2.0 .* m[ax..., 3:(end - 2)]); + dims = length(su)) A = AkimaInterpolation( u, t, nothing, b, c, d, extrapolate, cache_parameters, linear_lookup) I = cumulative_integral(A, cache_parameters) @@ -389,6 +427,32 @@ function QuadraticSpline( QuadraticSpline(u, t, I, p, tA, d, z, extrapolate, cache_parameters, linear_lookup) end +function QuadraticSpline( + u::uType, t; extrapolate = false, cache_parameters = false, + assume_linear_t = 1e-2) where {uType <: AbstractArray} + u, t = munge_data(u, t) + linear_lookup = seems_linear(assume_linear_t, t) + s = length(t) + dl = ones(eltype(t), s - 1) + d_tmp = ones(eltype(t), s) + du = zeros(eltype(t), s - 1) + tA = Tridiagonal(dl, d_tmp, du) + ax = axes(u)[1:(end - 1)] + d_ = map( + i -> i == 1 ? zeros(eltype(t), size(u[ax..., 1])) : + 2 // 1 * (u[ax..., i] - u[ax..., i - 1]) / (t[i] - t[i - 1]), + 1:s) + d = transpose(reshape(reduce(hcat, d_), :, s)) + z_ = reshape(transpose(tA \ d), size(u[ax..., 1])..., :) + z = [z_s for z_s in eachslice(z_, dims = ndims(z_))] + + p = QuadraticSplineParameterCache(z, t, cache_parameters) + A = QuadraticSpline( + u, t, nothing, p, tA, d, z, extrapolate, cache_parameters, linear_lookup) + I = cumulative_integral(A, cache_parameters) + QuadraticSpline(u, t, I, p, tA, d, z, extrapolate, cache_parameters, linear_lookup) +end + """ CubicSpline(u, t; extrapolate = false, cache_parameters = false) diff --git a/src/interpolation_methods.jl b/src/interpolation_methods.jl index 5223d416..591dd971 100644 --- a/src/interpolation_methods.jl +++ b/src/interpolation_methods.jl @@ -87,13 +87,15 @@ function _interpolate(A::LagrangeInterpolation{<:AbstractVector}, t::Number, igu N / D end -function _interpolate(A::LagrangeInterpolation{<:AbstractMatrix}, t::Number, iguess) +function _interpolate( + A::LagrangeInterpolation{<:AbstractArray}, t::Number, iguess) idx = get_idx(A, t, iguess) findRequiredIdxs!(A, t, idx) + ax = axes(A.u)[1:(end - 1)] if A.t[A.idxs[1]] == t - return A.u[:, A.idxs[1]] + return A.u[ax..., A.idxs[1]] end - N = zero(A.u[:, 1]) + N = zero(A.u[ax..., 1]) D = zero(A.t[1]) tmp = D for i in 1:length(A.idxs) @@ -111,7 +113,7 @@ function _interpolate(A::LagrangeInterpolation{<:AbstractMatrix}, t::Number, igu end tmp = inv((t - A.t[A.idxs[i]]) * mult) D += tmp - @. N += (tmp * A.u[:, A.idxs[i]]) + @. N += (tmp * A.u[ax..., A.idxs[i]]) end N / D end @@ -122,6 +124,13 @@ function _interpolate(A::AkimaInterpolation{<:AbstractVector}, t::Number, iguess @evalpoly wj A.u[idx] A.b[idx] A.c[idx] A.d[idx] end +function _interpolate(A::AkimaInterpolation{<:AbstractArray}, t::Number, iguess) + idx = get_idx(A, t, iguess) + wj = t - A.t[idx] + ax = axes(A.u)[1:(end - 1)] + @. @evalpoly wj A.u[ax..., idx] A.b[ax..., idx] A.c[ax..., idx] A.d[ax..., idx] +end + # ConstantInterpolation Interpolation function _interpolate(A::ConstantInterpolation{<:AbstractVector}, t::Number, iguess) if A.dir === :left @@ -134,7 +143,8 @@ function _interpolate(A::ConstantInterpolation{<:AbstractVector}, t::Number, igu A.u[idx] end -function _interpolate(A::ConstantInterpolation{<:AbstractMatrix}, t::Number, iguess) +function _interpolate( + A::ConstantInterpolation{<:AbstractArray}, t::Number, iguess) if A.dir === :left # :left means that value to the left is used for interpolation idx = get_idx(A, t, iguess; lb = 1, ub_shift = 0) @@ -142,7 +152,7 @@ function _interpolate(A::ConstantInterpolation{<:AbstractMatrix}, t::Number, igu # :right means that value to the right is used for interpolation idx = get_idx(A, t, iguess; side = :first, lb = 1, ub_shift = 0) end - A.u[:, idx] + A.u[axes(A.u)[1:(end - 1)]..., idx] end # QuadraticSpline Interpolation @@ -154,6 +164,16 @@ function _interpolate(A::QuadraticSpline{<:AbstractVector}, t::Number, iguess) return A.z[idx] * Δt + σ * Δt^2 + Cᵢ end +function _interpolate( + A::QuadraticSpline{<:AbstractArray}, t::Number, iguess) + idx = get_idx(A, t, iguess) + ax = axes(A.u)[1:(end - 1)] + Cᵢ = A.u[ax..., idx] + Δt = t - A.t[idx] + σ = get_parameters(A, idx) + return A.z[idx] * Δt + σ * Δt^2 + Cᵢ +end + # CubicSpline Interpolation function _interpolate(A::CubicSpline{<:AbstractVector}, t::Number, iguess) idx = get_idx(A, t, iguess) @@ -166,7 +186,7 @@ function _interpolate(A::CubicSpline{<:AbstractVector}, t::Number, iguess) I + C + D end -function _interpolate(A::CubicSpline{<:AbstractArray{T, N}}, t::Number, iguess) where {T, N} +function _interpolate(A::CubicSpline{<:AbstractArray}, t::Number, iguess) idx = get_idx(A, t, iguess) Δt₁ = t - A.t[idx] Δt₂ = A.t[idx + 1] - t @@ -225,6 +245,18 @@ function _interpolate( out end +function _interpolate( + A::CubicHermiteSpline{<:AbstractArray}, t::Number, iguess) + idx = get_idx(A, t, iguess) + Δt₀ = t - A.t[idx] + Δt₁ = t - A.t[idx + 1] + ax = axes(A.u)[1:(end - 1)] + out = A.u[ax..., idx] .+ Δt₀ .* A.du[ax..., idx] + c₁, c₂ = get_parameters(A, idx) + out .+= Δt₀^2 .* (c₁ .+ Δt₁ .* c₂) + out +end + # Quintic Hermite Spline function _interpolate( A::QuinticHermiteSpline{<:AbstractVector{<:Number}}, t::Number, iguess) @@ -236,3 +268,15 @@ function _interpolate( out += Δt₀^3 * (c₁ + Δt₁ * (c₂ + c₃ * Δt₁)) out end + +function _interpolate( + A::QuinticHermiteSpline{<:AbstractArray}, t::Number, iguess) + idx = get_idx(A, t, iguess) + Δt₀ = t - A.t[idx] + Δt₁ = t - A.t[idx + 1] + ax = axes(A.u)[1:(end - 1)] + out = A.u[ax..., idx] + Δt₀ * (A.du[ax..., idx] + A.ddu[ax..., idx] * Δt₀ / 2) + c₁, c₂, c₃ = get_parameters(A, idx) + out .+= Δt₀^3 .* (c₁ .+ Δt₁ .* (c₂ .+ c₃ .* Δt₁)) + out +end diff --git a/src/parameter_caches.jl b/src/parameter_caches.jl index 7c2d6f0b..ac09ac36 100644 --- a/src/parameter_caches.jl +++ b/src/parameter_caches.jl @@ -52,11 +52,12 @@ function QuadraticParameterCache(u, t, cache_parameters) end end -function quadratic_interpolation_parameters(u, t, idx) - if u isa AbstractMatrix - u₀ = u[:, idx] - u₁ = u[:, idx + 1] - u₂ = u[:, idx + 2] +function quadratic_interpolation_parameters(u::AbstractArray{T, N}, t, idx) where {T, N} + if N > 1 + ax = axes(u) + u₀ = u[ax[1:(end - 1)]..., idx] + u₁ = u[ax[1:(end - 1)]..., idx + 1] + u₂ = u[ax[1:(end - 1)]..., idx + 2] else u₀ = u[idx] u₁ = u[idx + 1] @@ -89,11 +90,17 @@ function QuadraticSplineParameterCache(z, t, cache_parameters) end end -function quadratic_spline_parameters(z, t, idx) +function quadratic_spline_parameters(z::AbstractVector, t, idx) σ = 1 // 2 * (z[idx + 1] - z[idx]) / (t[idx + 1] - t[idx]) return σ end +function quadratic_spline_parameters(z::AbstractArray, t, idx) + ax = axes(z)[1:(end - 1)] + σ = 1 // 2 * (z[ax..., idx + 1] - z[ax..., idx]) / (t[idx + 1] - t[idx]) + return σ +end + struct CubicSplineParameterCache{pType} c₁::pType c₂::pType @@ -145,7 +152,19 @@ function CubicHermiteParameterCache(du, u, t, cache_parameters) end end -function cubic_hermite_spline_parameters(du, u, t, idx) +function cubic_hermite_spline_parameters(du::AbstractArray, u, t, idx) + ax = axes(u)[1:(end - 1)] + Δt = t[idx + 1] - t[idx] + u₀ = u[ax..., idx] + u₁ = u[ax..., idx + 1] + du₀ = du[ax..., idx] + du₁ = du[ax..., idx + 1] + c₁ = (u₁ - u₀ - du₀ * Δt) / Δt^2 + c₂ = (du₁ - du₀ - 2c₁ * Δt) / Δt^2 + return c₁, c₂ +end + +function cubic_hermite_spline_parameters(du::AbstractVector, u, t, idx) Δt = t[idx + 1] - t[idx] u₀ = u[idx] u₁ = u[idx + 1] @@ -176,7 +195,7 @@ function QuinticHermiteParameterCache(ddu, du, u, t, cache_parameters) end end -function quintic_hermite_spline_parameters(ddu, du, u, t, idx) +function quintic_hermite_spline_parameters(ddu::AbstractVector, du, u, t, idx) Δt = t[idx + 1] - t[idx] u₀ = u[idx] u₁ = u[idx + 1] @@ -189,3 +208,18 @@ function quintic_hermite_spline_parameters(ddu, du, u, t, idx) c₃ = (6u₁ - 6u₀ - 3(du₀ + du₁)Δt + (ddu₁ - ddu₀)Δt^2 / 2) / Δt^5 return c₁, c₂, c₃ end + +function quintic_hermite_spline_parameters(ddu::AbstractArray, du, u, t, idx) + ax = axes(ddu)[1:(end - 1)] + Δt = t[idx + 1] - t[idx] + u₀ = u[ax..., idx] + u₁ = u[ax..., idx + 1] + du₀ = du[ax..., idx] + du₁ = du[ax..., idx + 1] + ddu₀ = ddu[ax..., idx] + ddu₁ = ddu[ax..., idx + 1] + c₁ = (u₁ - u₀ - du₀ * Δt - ddu₀ * Δt^2 / 2) / Δt^3 + c₂ = (3u₀ - 3u₁ + 2(du₀ + du₁ / 2)Δt + ddu₀ * Δt^2 / 2) / Δt^4 + c₃ = (6u₁ - 6u₀ - 3(du₀ + du₁)Δt + (ddu₁ - ddu₀)Δt^2 / 2) / Δt^5 + return c₁, c₂, c₃ +end