Introduce idx_prev to use as iguess

src/derivatives.jl

@@ -1,8 +1,20 @@
 function derivative(A, t, order = 1)
-    order > 2 && throw(DerivativeNotFoundError())
     ((t < A.t[1] || t > A.t[end]) && !A.extrapolate) && throw(ExtrapolationError())
-    order == 1 && return _derivative(A, t, firstindex(A.t) - 1)[1]
-    return ForwardDiff.derivative(t -> _derivative(A, t, firstindex(A.t) - 1)[1], t)
+    iguess = A.idx_prev[]
+
+    return if order == 1
+        val, idx = _derivative(A, t, iguess)
+        A.idx_prev[] = idx
+        val
+    elseif order == 2
+        ForwardDiff.derivative(t -> begin
+                val, idx = _derivative(A, t, iguess)
+                A.idx_prev[] = idx
+                val
+            end, t)
+    else
+        throw(DerivativeNotFoundError())
+    end
 end
 
 function _derivative(A::LinearInterpolation{<:AbstractVector}, t::Number, iguess)
@@ -105,17 +117,21 @@ function _derivative(A::LagrangeInterpolation{<:AbstractMatrix}, t::Number)
     der
 end
 
-_derivative(A::LagrangeInterpolation{<:AbstractVector}, t::Number, i) = _derivative(A, t), i
-_derivative(A::LagrangeInterpolation{<:AbstractMatrix}, t::Number, i) = _derivative(A, t), i
+function _derivative(A::LagrangeInterpolation{<:AbstractVector}, t::Number, idx)
+    _derivative(A, t), idx
+end
+function _derivative(A::LagrangeInterpolation{<:AbstractMatrix}, t::Number, idx)
+    _derivative(A, t), idx
+end
 
 function _derivative(A::AkimaInterpolation{<:AbstractVector}, t::Number, iguess)
-    i = searchsortedfirstcorrelated(A.t, t, iguess)
-    i > length(A.t) ? i -= 1 : nothing
-    i -= 1
-    i == 0 ? i += 1 : nothing
-    j = min(i, length(A.c))  # for smooth derivative at A.t[end]
-    wj = t - A.t[i]
-    (@evalpoly wj A.b[i] 2A.c[j] 3A.d[j]), i
+    idx = searchsortedfirstcorrelated(A.t, t, iguess)
+    idx > length(A.t) ? idx -= 1 : nothing
+    idx -= 1
+    idx == 0 ? idx += 1 : nothing
+    j = min(idx, length(A.c))  # for smooth derivative at A.t[end]
+    wj = t - A.t[idx]
+    (@evalpoly wj A.b[idx] 2A.c[j] 3A.d[j]), idx
 end
 
 function _derivative(A::ConstantInterpolation{<:AbstractVector}, t::Number)
@@ -139,15 +155,15 @@ end
 
 # CubicSpline Interpolation
 function _derivative(A::CubicSpline{<:AbstractVector}, t::Number, iguess)
-    i = searchsortedfirstcorrelated(A.t, t, iguess)
-    i > length(A.t) ? i -= 1 : nothing
-    i -= 1
-    i == 0 ? i += 1 : nothing
-    dI = -3A.z[i] * (A.t[i + 1] - t)^2 / (6A.h[i + 1]) +
-         3A.z[i + 1] * (t - A.t[i])^2 / (6A.h[i + 1])
-    dC = A.u[i + 1] / A.h[i + 1] - A.z[i + 1] * A.h[i + 1] / 6
-    dD = -(A.u[i] / A.h[i + 1] - A.z[i] * A.h[i + 1] / 6)
-    dI + dC + dD, i
+    idx = searchsortedfirstcorrelated(A.t, t, iguess)
+    idx > length(A.t) ? idx -= 1 : nothing
+    idx -= 1
+    idx == 0 ? idx += 1 : nothing
+    dI = -3A.z[idx] * (A.t[idx + 1] - t)^2 / (6A.h[idx + 1]) +
+         3A.z[idx + 1] * (t - A.t[idx])^2 / (6A.h[idx + 1])
+    dC = A.u[idx + 1] / A.h[idx + 1] - A.z[idx + 1] * A.h[idx + 1] / 6
+    dD = -(A.u[idx] / A.h[idx + 1] - A.z[idx] * A.h[idx + 1] / 6)
+    dI + dC + dD, idx
 end
 
 function _derivative(A::BSplineInterpolation{<:AbstractVector{<:Number}}, t::Number, iguess)

src/interpolation_caches.jl

@@ -17,8 +17,9 @@ struct LinearInterpolation{uType, tType, T} <: AbstractInterpolation{T}
     u::uType
     t::tType
     extrapolate::Bool
+    idx_prev::Base.RefValue{Int}
     function LinearInterpolation(u, t, extrapolate)
-        new{typeof(u), typeof(t), eltype(u)}(u, t, extrapolate)
+        new{typeof(u), typeof(t), eltype(u)}(u, t, extrapolate, Ref(1))
     end
 end
 
@@ -48,10 +49,11 @@ struct QuadraticInterpolation{uType, tType, T} <: AbstractInterpolation{T}
     t::tType
     mode::Symbol
     extrapolate::Bool
+    idx_prev::Base.RefValue{Int}
     function QuadraticInterpolation(u, t, mode, extrapolate)
         mode ∈ (:Forward, :Backward) ||
             error("mode should be :Forward or :Backward for QuadraticInterpolation")
-        new{typeof(u), typeof(t), eltype(u)}(u, t, mode, extrapolate)
+        new{typeof(u), typeof(t), eltype(u)}(u, t, mode, extrapolate, Ref(1))
     end
 end
 
@@ -86,14 +88,17 @@ struct LagrangeInterpolation{uType, tType, T, bcacheType} <:
     n::Int
     bcache::bcacheType
     extrapolate::Bool
+    idx_prev::Base.RefValue{Int}
     function LagrangeInterpolation(u, t, n, extrapolate)
         bcache = zeros(eltype(u[1]), n + 1)
         fill!(bcache, NaN)
         new{typeof(u), typeof(t), eltype(u), typeof(bcache)}(u,
             t,
             n,
             bcache,
-            extrapolate)
+            extrapolate,
+            Ref(1)
+        )
     end
 end
 
@@ -128,14 +133,17 @@ struct AkimaInterpolation{uType, tType, bType, cType, dType, T} <:
     c::cType
     d::dType
     extrapolate::Bool
+    idx_prev::Base.RefValue{Int}
     function AkimaInterpolation(u, t, b, c, d, extrapolate)
         new{typeof(u), typeof(t), typeof(b), typeof(c),
             typeof(d), eltype(u)}(u,
             t,
             b,
             c,
             d,
-            extrapolate)
+            extrapolate,
+            Ref(1)
+        )
     end
 end
 
@@ -186,8 +194,9 @@ struct ConstantInterpolation{uType, tType, dirType, T} <: AbstractInterpolation{
     t::tType
     dir::Symbol # indicates if value to the $dir should be used for the interpolation
     extrapolate::Bool
+    idx_prev::Base.RefValue{Int}
     function ConstantInterpolation(u, t, dir, extrapolate)
-        new{typeof(u), typeof(t), typeof(dir), eltype(u)}(u, t, dir, extrapolate)
+        new{typeof(u), typeof(t), typeof(dir), eltype(u)}(u, t, dir, extrapolate, Ref(1))
     end
 end
 
@@ -219,14 +228,17 @@ struct QuadraticSpline{uType, tType, tAType, dType, zType, T} <:
     d::dType
     z::zType
     extrapolate::Bool
+    idx_prev::Base.RefValue{Int}
     function QuadraticSpline(u, t, tA, d, z, extrapolate)
         new{typeof(u), typeof(t), typeof(tA),
             typeof(d), typeof(z), eltype(u)}(u,
             t,
             tA,
             d,
             z,
-            extrapolate)
+            extrapolate,
+            Ref(1)
+        )
     end
 end
 
@@ -286,12 +298,15 @@ struct CubicSpline{uType, tType, hType, zType, T} <: AbstractInterpolation{T}
     h::hType
     z::zType
     extrapolate::Bool
+    idx_prev::Base.RefValue{Int}
     function CubicSpline(u, t, h, z, extrapolate)
         new{typeof(u), typeof(t), typeof(h), typeof(z), eltype(u)}(u,
             t,
             h,
             z,
-            extrapolate)
+            extrapolate,
+            Ref(1)
+        )
     end
 end
 
@@ -365,6 +380,7 @@ struct BSplineInterpolation{uType, tType, pType, kType, cType, T} <:
     pVecType::Symbol
     knotVecType::Symbol
     extrapolate::Bool
+    idx_prev::Base.RefValue{Int}
     function BSplineInterpolation(u,
             t,
             d,
@@ -382,7 +398,9 @@ struct BSplineInterpolation{uType, tType, pType, kType, cType, T} <:
             c,
             pVecType,
             knotVecType,
-            extrapolate)
+            extrapolate,
+            Ref(1)
+        )
     end
 end
 
@@ -482,6 +500,7 @@ struct BSplineApprox{uType, tType, pType, kType, cType, T} <:
     pVecType::Symbol
     knotVecType::Symbol
     extrapolate::Bool
+    idx_prev::Base.RefValue{Int}
     function BSplineApprox(u,
             t,
             d,
@@ -501,7 +520,9 @@ struct BSplineApprox{uType, tType, pType, kType, cType, T} <:
             c,
             pVecType,
             knotVecType,
-            extrapolate)
+            extrapolate,
+            Ref(1)
+        )
     end
 end
 

src/interpolation_methods.jl

@@ -1,7 +1,9 @@
-function _interpolate(interp, t)
-    ((t < interp.t[1] || t > interp.t[end]) && !interp.extrapolate) &&
+function _interpolate(A, t)
+    ((t < A.t[1] || t > A.t[end]) && !A.extrapolate) &&
         throw(ExtrapolationError())
-    _interpolate(interp, t, firstindex(interp.t) - 1)[1]
+    val, idx_prev = _interpolate(A, t, A.idx_prev[])
+    A.idx_prev[] = idx_prev
+    return val
 end
 
 # Linear Interpolation
@@ -114,61 +116,62 @@ function _interpolate(A::LagrangeInterpolation{<:AbstractMatrix}, t::Number)
     N / D
 end
 
-function _interpolate(A::LagrangeInterpolation{<:AbstractVector}, t::Number, i)
-    _interpolate(A, t), i
+function _interpolate(A::LagrangeInterpolation{<:AbstractVector}, t::Number, idx)
+    _interpolate(A, t), idx
 end
 
-function _interpolate(A::LagrangeInterpolation{<:AbstractMatrix}, t::Number, i)
-    _interpolate(A, t), i
+function _interpolate(A::LagrangeInterpolation{<:AbstractMatrix}, t::Number, idx)
+    _interpolate(A, t), idx
 end
 
 function _interpolate(A::AkimaInterpolation{<:AbstractVector}, t::Number, iguess)
-    i = max(1, min(searchsortedlastcorrelated(A.t, t, iguess), length(A.t) - 1))
-    wj = t - A.t[i]
-    (@evalpoly wj A.u[i] A.b[i] A.c[i] A.d[i]), i
+    idx = max(1, min(searchsortedlastcorrelated(A.t, t, iguess), length(A.t) - 1))
+    wj = t - A.t[idx]
+    (@evalpoly wj A.u[idx] A.b[idx] A.c[idx] A.d[idx]), idx
 end
 
 # ConstantInterpolation Interpolation
 function _interpolate(A::ConstantInterpolation{<:AbstractVector}, t::Number, iguess)
     if A.dir === :left
         # :left means that value to the left is used for interpolation
-        i = max(1, searchsortedlastcorrelated(A.t, t, iguess))
-        return A.u[i], i
+        idx = max(1, searchsortedlastcorrelated(A.t, t, iguess))
+        return A.u[idx], idx
     else
         # :right means that value to the right is used for interpolation
-        i = min(length(A.t), searchsortedfirstcorrelated(A.t, t, iguess))
-        return A.u[i], i
+        idx = min(length(A.t), searchsortedfirstcorrelated(A.t, t, iguess))
+        return A.u[idx], idx
     end
 end
 
 function _interpolate(A::ConstantInterpolation{<:AbstractMatrix}, t::Number, iguess)
     if A.dir === :left
         # :left means that value to the left is used for interpolation
-        i = max(1, searchsortedlastcorrelated(A.t, t, iguess))
-        return A.u[:, i], i
+        idx = max(1, searchsortedlastcorrelated(A.t, t, iguess))
+        return A.u[:, idx], idx
     else
         # :right means that value to the right is used for interpolation
-        i = min(length(A.t), searchsortedfirstcorrelated(A.t, t, iguess))
-        return A.u[:, i], i
+        idx = min(length(A.t), searchsortedfirstcorrelated(A.t, t, iguess))
+        return A.u[:, idx], idx
     end
 end
 
 # QuadraticSpline Interpolation
 function _interpolate(A::QuadraticSpline{<:AbstractVector}, t::Number, iguess)
-    i = min(max(2, searchsortedfirstcorrelated(A.t, t, iguess)), length(A.t))
-    Cᵢ = A.u[i - 1]
-    σ = 1 // 2 * (A.z[i] - A.z[i - 1]) / (A.t[i] - A.t[i - 1])
-    return A.z[i - 1] * (t - A.t[i - 1]) + σ * (t - A.t[i - 1])^2 + Cᵢ, i
+    idx = min(max(2, searchsortedfirstcorrelated(A.t, t, iguess)), length(A.t))
+    Cᵢ = A.u[idx - 1]
+    σ = 1 // 2 * (A.z[idx] - A.z[idx - 1]) / (A.t[idx] - A.t[idx - 1])
+    return A.z[idx - 1] * (t - A.t[idx - 1]) + σ * (t - A.t[idx - 1])^2 + Cᵢ, idx
 end
 
 # CubicSpline Interpolation
 function _interpolate(A::CubicSpline{<:AbstractVector}, t::Number, iguess)
-    i = max(1, min(searchsortedlastcorrelated(A.t, t, iguess), length(A.t) - 1))
-    I = A.z[i] * (A.t[i + 1] - t)^3 / (6A.h[i + 1]) +
-        A.z[i + 1] * (t - A.t[i])^3 / (6A.h[i + 1])
-    C = (A.u[i + 1] / A.h[i + 1] - A.z[i + 1] * A.h[i + 1] / 6) * (t - A.t[i])
-    D = (A.u[i] / A.h[i + 1] - A.z[i] * A.h[i + 1] / 6) * (A.t[i + 1] - t)
-    I + C + D, i
+    idx = max(1, min(searchsortedlastcorrelated(A.t, t, iguess), length(A.t) - 1))
+    A.idx_prev[] = idx
+    I = A.z[idx] * (A.t[idx + 1] - t)^3 / (6A.h[idx + 1]) +
+        A.z[idx + 1] * (t - A.t[idx])^3 / (6A.h[idx + 1])
+    C = (A.u[idx + 1] / A.h[idx + 1] - A.z[idx + 1] * A.h[idx + 1] / 6) * (t - A.t[idx])
+    D = (A.u[idx] / A.h[idx + 1] - A.z[idx] * A.h[idx + 1] / 6) * (A.t[idx + 1] - t)
+    I + C + D, idx
 end
 
 # BSpline Curve Interpolation

src/interpolation_utils.jl

@@ -41,11 +41,12 @@ function munge_data(u::AbstractVector, t::AbstractVector)
     Tu = Base.nonmissingtype(eltype(u))
     Tt = Base.nonmissingtype(eltype(t))
     @assert length(t) == length(u)
-    non_missing_indices = collect(i
-    for i in 1:length(t)
-    if !ismissing(u[i]) && !ismissing(t[i]))
-    newu = Tu.([u[i] for i in non_missing_indices])
-    newt = Tt.([t[i] for i in non_missing_indices])
+    non_missing_indices = collect(
+        idx for idx in 1:length(t)
+    if !ismissing(u[idx]) && !ismissing(t[idx])
+    )
+    newu = Tu.([u[idx] for idx in non_missing_indices])
+    newt = Tt.([t[idx] for idx in non_missing_indices])
 
     return newu, newt
 end
@@ -54,11 +55,12 @@ function munge_data(U::StridedMatrix, t::AbstractVector)
     TU = Base.nonmissingtype(eltype(U))
     Tt = Base.nonmissingtype(eltype(t))
     @assert length(t) == size(U, 2)
-    non_missing_indices = collect(i
-    for i in 1:length(t)
-    if !any(ismissing, U[:, i]) && !ismissing(t[i]))
-    newUs = [TU.(U[:, i]) for i in non_missing_indices]
-    newt = Tt.([t[i] for i in non_missing_indices])
+    non_missing_indices = collect(
+        idx for idx in 1:length(t)
+    if !any(ismissing, U[:, idx]) && !ismissing(t[idx])
+    )
+    newUs = [TU.(U[:, idx]) for idx in non_missing_indices]
+    newt = Tt.([t[idx] for idx in non_missing_indices])
 
     return hcat(newUs...), newt
 end