From 430e87d8aef38f057f66b89002c61cfb29950a97 Mon Sep 17 00:00:00 2001 From: Ashutosh Bharambe Date: Sat, 26 Oct 2024 06:54:34 +0000 Subject: [PATCH 1/3] feat(BSplines): add methods for Vector{Vector} --- src/interpolation_caches.jl | 187 ++++++++++++++++++++++++++++++++++- src/interpolation_methods.jl | 32 ++++++ 2 files changed, 218 insertions(+), 1 deletion(-) diff --git a/src/interpolation_caches.jl b/src/interpolation_caches.jl index 37da05f2..ae54dea7 100644 --- a/src/interpolation_caches.jl +++ b/src/interpolation_caches.jl @@ -738,6 +738,78 @@ function BSplineInterpolation( u, t, d, p, k, c, sc, pVecType, knotVecType, extrapolate, assume_linear_t) end +function BSplineInterpolation( + u::AbstractVector{<:AbstractVector{T}}, t, d, pVecType, knotVecType; extrapolate = false, + assume_linear_t = 1e-2) where {T <: Number} + 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(T) + p = zero(t) + k = zeros(eltype(t), n + d + 1) + l = zeros(T, 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[i] - 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 \ reduce(hcat, u)')' + c = collect(eachcol(c)) + 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) @@ -900,9 +972,106 @@ function BSplineApprox( u, t, d, h, p, k, c, sc, pVecType, knotVecType, extrapolate, assume_linear_t) end +function BSplineApprox( + u::AbstractVector{<:AbstractVector{T}}, t, d, h, pVecType, knotVecType; extrapolate = false, + assume_linear_t = 1e-2) where {T} + u, t = munge_data(u, t) + n = length(t) + h < d + 1 && error("BSplineApprox needs at least d + 1, i.e. $(d+1) control points.") + s = zero(T) + p = zero(t) + k = zeros(eltype(t), h + d + 1) + l = zeros(T, 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[i] - 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):h + k[i] = k[1] + (i - d - 1) // (h - d) * (k[end] - k[1]) + end + elseif knotVecType == :Average + # NOTE: verify that average method can be applied when size of k is less than size of p + # average spaced knot vector + idx = 1 + if d + 2 <= h + k[d + 2] = 1 // d * ps[d] + end + for i in (d + 3):h + k[i] = 1 // d * (ps[idx + d] - ps[idx]) + idx += 1 + end + end + # control points + c = zeros(T, length(u[1]), h) + c[:, 1] = u[1] + c[:, end] = u[end] + q = zeros(T, length(u[1]), n) + sc = zeros(eltype(t), n, h) + for i in 1:n + spline_coefficients!(view(sc, i, :), d, k, p[i]) + end + for k in 2:(n - 1) + q[:, k] = u[k] - sc[k, 1] * u[1] - + sc[k, h] * u[end] + end + Q = Matrix{T}(undef, length(u[1]), h - 2) + for i in 2:(h - 1) + s = zeros(eltype(sc), length(u[1])) + for k in 2:(n - 1) + s = s + sc[k, i] .* q[:, k] + end + Q[:, i - 1] = s + end + sc = sc[2:(end - 1), 2:(h - 1)] + M = transpose(sc) * sc + Q = reshape(Q, length(u[1]), :) + + P = (M \ Q')' + P = reshape(P, length(u[1]), :) + c[:, 2:(end - 1)] = P + sc = zeros(eltype(t), h) + BSplineApprox( + u, t, d, h, p, k, eachcol(c), sc, pVecType, knotVecType, extrapolate, assume_linear_t) +end + function BSplineApprox( u::AbstractArray{T, N}, t, d, h, pVecType, knotVecType; extrapolate = false, - assume_linear_t = 1e-2) where {T, N} + assume_linear_t = 1e-2) where {T <: Number, N} u, t = munge_data(u, t) n = length(t) h < d + 1 && error("BSplineApprox needs at least d + 1, i.e. $(d+1) control points.") @@ -945,6 +1114,22 @@ function BSplineApprox( ps[i - 1] = s 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, diff --git a/src/interpolation_methods.jl b/src/interpolation_methods.jl index c429ff1f..2b321d76 100644 --- a/src/interpolation_methods.jl +++ b/src/interpolation_methods.jl @@ -216,6 +216,23 @@ function _interpolate(A::BSplineInterpolation{<:AbstractArray{T, N}}, ucum end +function _interpolate(A::BSplineInterpolation{<:AbstractVector{<:AbstractVector{T}}}, + t::Number, + iguess) where {T <: Number} + t < A.t[1] && return A.u[1] + t > A.t[end] && return A.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(T, size(A.u[1])) + for i in nonzero_coefficient_idxs + ucum += sc[i] * A.c[i] + end + ucum +end # BSpline Curve Approx function _interpolate(A::BSplineApprox{<:AbstractVector{<:Number}}, t::Number, iguess) t < A.t[1] && return A.u[1] @@ -249,6 +266,21 @@ function _interpolate( ucum end +function _interpolate(A::BSplineApprox{<:AbstractVector{<:AbstractVector{T}}}, + t::Number, iguess) where {T <: Number} + t < A.t[1] && return A.u[1] + t > A.t[end] && return A.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]) + sc = t isa ForwardDiff.Dual ? zeros(eltype(t), A.h) : A.sc + nonzero_coefficient_idxs = spline_coefficients!(sc, A.d, A.k, t) + ucum = zeros(T, size(A.u[1])) + for i in nonzero_coefficient_idxs + ucum += sc[i] * A.c[i] + end + ucum +end # Cubic Hermite Spline function _interpolate( A::CubicHermiteSpline{<:AbstractVector{<:Number}}, t::Number, iguess) From 315dc9d342db17b4836edb320df783fa7e3f1d2b Mon Sep 17 00:00:00 2001 From: Ashutosh Bharambe Date: Sat, 26 Oct 2024 06:55:06 +0000 Subject: [PATCH 2/3] test(BSplines): add tests --- test/interpolation_tests.jl | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/test/interpolation_tests.jl b/test/interpolation_tests.jl index 9af43d37..3f13973f 100644 --- a/test/interpolation_tests.jl +++ b/test/interpolation_tests.jl @@ -664,6 +664,8 @@ end @test_throws DataInterpolations.ExtrapolationError A(-1.0) @test_throws DataInterpolations.ExtrapolationError A(300.0) + @testset "Vector{Vector}" begin end + @testset "AbstractMatrix" begin t = 0.1:0.1:1.0 u2d = [sin.(t) cos.(t)]' |> collect @@ -721,6 +723,21 @@ end @test_throws DataInterpolations.ExtrapolationError A(-1.0) @test_throws DataInterpolations.ExtrapolationError A(300.0) + @testset "Vector{Vector}" begin + t = 0.1:0.1:1.0 + u_vec = [[sin.(t_), cos.(t_)] for t_ in t] + A = BSplineApprox(u_vec, t, 2, 5, :Uniform, :Uniform) + t_test = 0.1:0.05:1.0 + u_test = reduce(hcat, A.(t_test)) + @test isapprox(u_test[1, :], sin.(t_test), atol = 1e-3) + @test isapprox(u_test[2, :], cos.(t_test), atol = 1e-3) + + A = BSplineApprox(u_vec, t, 2, 5, :ArcLen, :Average) + u_test = reduce(hcat, A.(t_test)) + @test isapprox(u_test[1, :], sin.(t_test), atol = 1e-2) + @test isapprox(u_test[2, :], cos.(t_test), atol = 1e-2) + end + @testset "AbstractMatrix" begin t = 0.1:0.1:1.0 u2d = [sin.(t) cos.(t)]' |> collect From e71c5521805db2e40aacd2be4b17651b22dd15b1 Mon Sep 17 00:00:00 2001 From: Ashutosh Bharambe Date: Sat, 26 Oct 2024 08:42:45 +0000 Subject: [PATCH 3/3] test(BSplines): add tests --- test/interpolation_tests.jl | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/test/interpolation_tests.jl b/test/interpolation_tests.jl index 3f13973f..a8021640 100644 --- a/test/interpolation_tests.jl +++ b/test/interpolation_tests.jl @@ -664,7 +664,20 @@ end @test_throws DataInterpolations.ExtrapolationError A(-1.0) @test_throws DataInterpolations.ExtrapolationError A(300.0) - @testset "Vector{Vector}" begin end + @testset "Vector{Vector}" begin + t = 0.1:0.1:1.0 + u_vec = [[sin.(t_), cos.(t_)] for t_ in t] + A = BSplineInterpolation(u_vec, t, 2, :Uniform, :Uniform) + t_test = 0.1:0.05:1.0 + u_test = reduce(hcat, A.(t_test)) + @test isapprox(u_test[1, :], sin.(t_test), atol = 1e-3) + @test isapprox(u_test[2, :], cos.(t_test), atol = 1e-3) + + A = BSplineInterpolation(u_vec, t, 2, :ArcLen, :Average) + u_test = reduce(hcat, A.(t_test)) + @test isapprox(u_test[1, :], sin.(t_test), atol = 1e-2) + @test isapprox(u_test[2, :], cos.(t_test), atol = 1e-2) + end @testset "AbstractMatrix" begin t = 0.1:0.1:1.0