Introduce idx_prev to use as iguess

src/derivatives.jl

@@ -1,23 +1,29 @@
 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)
-    idx = searchsortedfirstcorrelated(A.t, t, iguess)
-    idx > length(A.t) ? idx -= 1 : nothing
-    idx -= 1
-    idx == 0 ? idx += 1 : nothing
+    idx = get_idx(A.t, t, iguess; idx_shift = -1, ub_shift = -2, side = :first)
     (A.u[idx + 1] - A.u[idx]) / (A.t[idx + 1] - A.t[idx]), idx
 end
 
 function _derivative(A::LinearInterpolation{<:AbstractMatrix}, t::Number, iguess)
-    idx = searchsortedfirstcorrelated(A.t, t, iguess)
-    idx > length(A.t) ? idx -= 1 : nothing
-    idx -= 1
-    idx == 0 ? idx += 1 : nothing
+    idx = get_idx(A.t, t, iguess; idx_shift = -1, ub_shift = -2, side = :first)
     (@views @. (A.u[:, idx + 1] - A.u[:, idx]) / (A.t[idx + 1] - A.t[idx])), idx
 end
 
@@ -105,17 +111,18 @@ 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 = get_idx(A.t, 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]
+    (@evalpoly wj A.b[idx] 2A.c[j] 3A.d[j]), idx
 end
 
 function _derivative(A::ConstantInterpolation{<:AbstractVector}, t::Number)
@@ -130,32 +137,26 @@ end
 
 # QuadraticSpline Interpolation
 function _derivative(A::QuadraticSpline{<:AbstractVector}, t::Number, iguess)
-    idx = searchsortedfirstcorrelated(A.t, t, iguess)
-    idx > length(A.t) ? idx -= 1 : nothing
-    idx == 1 ? idx += 1 : nothing
+    idx = get_idx(A.t, t, iguess; lb = 2, ub_shift = 0, side = :first)
     σ = 1 // 2 * (A.z[idx] - A.z[idx - 1]) / (A.t[idx] - A.t[idx - 1])
     A.z[idx - 1] + 2σ * (t - A.t[idx - 1]), idx
 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 = get_idx(A.t, t, iguess)
+    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)
     # change t into param [0 1]
     t < A.t[1] && return zero(A.u[1]), 1
     t > A.t[end] && return zero(A.u[end]), lastindex(t)
-    idx = searchsortedlastcorrelated(A.t, t, iguess)
-    idx == length(A.t) ? idx -= 1 : nothing
+    idx = get_idx(A.t, t, iguess)
     n = length(A.t)
     scale = (A.p[idx + 1] - A.p[idx]) / (A.t[idx + 1] - A.t[idx])
     t_ = A.p[idx] + (t - A.t[idx]) * scale
@@ -176,8 +177,7 @@ function _derivative(A::BSplineApprox{<:AbstractVector{<:Number}}, t::Number, ig
     # change t into param [0 1]
     t < A.t[1] && return zero(A.u[1]), 1
     t > A.t[end] && return zero(A.u[end]), lastindex(t)
-    idx = searchsortedlastcorrelated(A.t, t, iguess)
-    idx == length(A.t) ? idx -= 1 : nothing
+    idx = get_idx(A.t, t, iguess)
     scale = (A.p[idx + 1] - A.p[idx]) / (A.t[idx + 1] - A.t[idx])
     t_ = A.p[idx] + (t - A.t[idx]) * scale
     N = spline_coefficients(A.h, A.d - 1, A.k, t_)

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