1 /* ----------------------------------------------------------------------
2 * Project: CMSIS DSP Library
3 * Title: arm_spline_interp_f32.c
4 * Description: Floating-point cubic spline interpolation
5 *
6 * $Date: 23 April 2021
7 * $Revision: V1.9.0
8 *
9 * Target Processor: Cortex-M and Cortex-A cores
10 * -------------------------------------------------------------------- */
11 /*
12 * Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
13 *
14 * SPDX-License-Identifier: Apache-2.0
15 *
16 * Licensed under the Apache License, Version 2.0 (the License); you may
17 * not use this file except in compliance with the License.
18 * You may obtain a copy of the License at
19 *
20 * www.apache.org/licenses/LICENSE-2.0
21 *
22 * Unless required by applicable law or agreed to in writing, software
23 * distributed under the License is distributed on an AS IS BASIS, WITHOUT
24 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
25 * See the License for the specific language governing permissions and
26 * limitations under the License.
27 */
28
29 #include "dsp/interpolation_functions.h"
30
31 /**
32 @ingroup groupInterpolation
33 */
34
35 /**
36 @defgroup SplineInterpolate Cubic Spline Interpolation
37
38 Spline interpolation is a method of interpolation where the interpolant
39 is a piecewise-defined polynomial called "spline".
40
41 @par Introduction
42
43 Given a function f defined on the interval [a,b], a set of n nodes x(i)
44 where a=x(1)<x(2)<...<x(n)=b and a set of n values y(i) = f(x(i)),
45 a cubic spline interpolant S(x) is defined as:
46
47 <pre>
48 S1(x) x(1) < x < x(2)
49 S(x) = ...
50 Sn-1(x) x(n-1) < x < x(n)
51 </pre>
52
53 where
54
55 <pre>
56 Si(x) = a_i+b_i(x-xi)+c_i(x-xi)^2+d_i(x-xi)^3 i=1, ..., n-1
57 </pre>
58
59 @par Algorithm
60
61 Having defined h(i) = x(i+1) - x(i)
62
63 <pre>
64 h(i-1)c(i-1)+2[h(i-1)+h(i)]c(i)+h(i)c(i+1) = 3/h(i)*[a(i+1)-a(i)]-3/h(i-1)*[a(i)-a(i-1)] i=2, ..., n-1
65 </pre>
66
67 It is possible to write the previous conditions in matrix form (Ax=B).
68 In order to solve the system two boundary conidtions are needed.
69 - Natural spline: S1''(x1)=2*c(1)=0 ; Sn''(xn)=2*c(n)=0
70 In matrix form:
71
72 <pre>
73 | 1 0 0 ... 0 0 0 || c(1) | | 0 |
74 | h(0) 2[h(0)+h(1)] h(1) ... 0 0 0 || c(2) | | 3/h(2)*[a(3)-a(2)]-3/h(1)*[a(2)-a(1)] |
75 | ... ... ... ... ... ... ... || ... |=| ... |
76 | 0 0 0 ... h(n-2) 2[h(n-2)+h(n-1)] h(n-1) || c(n-1) | | 3/h(n-1)*[a(n)-a(n-1)]-3/h(n-2)*[a(n-1)-a(n-2)] |
77 | 0 0 0 ... 0 0 1 || c(n) | | 0 |
78 </pre>
79
80 - Parabolic runout spline: S1''(x1)=2*c(1)=S2''(x2)=2*c(2) ; Sn-1''(xn-1)=2*c(n-1)=Sn''(xn)=2*c(n)
81 In matrix form:
82
83 <pre>
84 | 1 -1 0 ... 0 0 0 || c(1) | | 0 |
85 | h(0) 2[h(0)+h(1)] h(1) ... 0 0 0 || c(2) | | 3/h(2)*[a(3)-a(2)]-3/h(1)*[a(2)-a(1)] |
86 | ... ... ... ... ... ... ... || ... |=| ... |
87 | 0 0 0 ... h(n-2) 2[h(n-2)+h(n-1)] h(n-1) || c(n-1) | | 3/h(n-1)*[a(n)-a(n-1)]-3/h(n-2)*[a(n-1)-a(n-2)] |
88 | 0 0 0 ... 0 -1 1 || c(n) | | 0 |
89 </pre>
90
91 A is a tridiagonal matrix (a band matrix of bandwidth 3) of size N=n+1. The factorization
92 algorithms (A=LU) can be simplified considerably because a large number of zeros appear
93 in regular patterns. The Crout method has been used:
94 1) Solve LZ=B
95
96 <pre>
97 u(1,2) = A(1,2)/A(1,1)
98 z(1) = B(1)/l(11)
99
100 FOR i=2, ..., N-1
101 l(i,i) = A(i,i)-A(i,i-1)u(i-1,i)
102 u(i,i+1) = a(i,i+1)/l(i,i)
103 z(i) = [B(i)-A(i,i-1)z(i-1)]/l(i,i)
104
105 l(N,N) = A(N,N)-A(N,N-1)u(N-1,N)
106 z(N) = [B(N)-A(N,N-1)z(N-1)]/l(N,N)
107 </pre>
108
109 2) Solve UX=Z
110
111 <pre>
112 c(N)=z(N)
113
114 FOR i=N-1, ..., 1
115 c(i)=z(i)-u(i,i+1)c(i+1)
116 </pre>
117
118 c(i) for i=1, ..., n-1 are needed to compute the n-1 polynomials.
119 b(i) and d(i) are computed as:
120 - b(i) = [y(i+1)-y(i)]/h(i)-h(i)*[c(i+1)+2*c(i)]/3
121 - d(i) = [c(i+1)-c(i)]/[3*h(i)]
122 Moreover, a(i)=y(i).
123
124 @par Behaviour outside the given intervals
125
126 It is possible to compute the interpolated vector for x values outside the
127 input range (xq<x(1); xq>x(n)). The coefficients used to compute the y values for
128 xq<x(1) are going to be the ones used for the first interval, while for xq>x(n) the
129 coefficients used for the last interval.
130
131 */
132
133 /**
134 @addtogroup SplineInterpolate
135 @{
136 */
137
138 /**
139 * @brief Processing function for the floating-point cubic spline interpolation.
140 * @param[in] S points to an instance of the floating-point spline structure.
141 * @param[in] xq points to the x values of the interpolated data points.
142 * @param[out] pDst points to the block of output data.
143 * @param[in] blockSize number of samples of output data.
144 */
145
arm_spline_f32(arm_spline_instance_f32 * S,const float32_t * xq,float32_t * pDst,uint32_t blockSize)146 void arm_spline_f32(
147 arm_spline_instance_f32 * S,
148 const float32_t * xq,
149 float32_t * pDst,
150 uint32_t blockSize)
151 {
152 const float32_t * x = S->x;
153 const float32_t * y = S->y;
154 int32_t n = S->n_x;
155
156 /* Coefficients (a==y for i<=n-1) */
157 float32_t * b = (S->coeffs);
158 float32_t * c = (S->coeffs)+(n-1);
159 float32_t * d = (S->coeffs)+(2*(n-1));
160
161 const float32_t * pXq = xq;
162 int32_t blkCnt = (int32_t)blockSize;
163 int32_t blkCnt2;
164 int32_t i;
165 float32_t x_sc;
166
167 #ifdef ARM_MATH_NEON
168 float32x4_t xiv;
169 float32x4_t aiv;
170 float32x4_t biv;
171 float32x4_t civ;
172 float32x4_t div;
173
174 float32x4_t xqv;
175
176 float32x4_t temp;
177 float32x4_t diff;
178 float32x4_t yv;
179 #endif
180
181 /* Create output for x(i)<x<x(i+1) */
182 for (i=0; i<n-1; i++)
183 {
184 #ifdef ARM_MATH_NEON
185 xiv = vdupq_n_f32(x[i]);
186
187 aiv = vdupq_n_f32(y[i]);
188 biv = vdupq_n_f32(b[i]);
189 civ = vdupq_n_f32(c[i]);
190 div = vdupq_n_f32(d[i]);
191
192 while( *(pXq+4) <= x[i+1] && blkCnt > 4 )
193 {
194 /* Load [xq(k) xq(k+1) xq(k+2) xq(k+3)] */
195 xqv = vld1q_f32(pXq);
196 pXq+=4;
197
198 /* Compute [xq(k)-x(i) xq(k+1)-x(i) xq(k+2)-x(i) xq(k+3)-x(i)] */
199 diff = vsubq_f32(xqv, xiv);
200 temp = diff;
201
202 /* y(i) = a(i) + ... */
203 yv = aiv;
204 /* ... + b(i)*(x-x(i)) + ... */
205 yv = vmlaq_f32(yv, biv, temp);
206 /* ... + c(i)*(x-x(i))^2 + ... */
207 temp = vmulq_f32(temp, diff);
208 yv = vmlaq_f32(yv, civ, temp);
209 /* ... + d(i)*(x-x(i))^3 */
210 temp = vmulq_f32(temp, diff);
211 yv = vmlaq_f32(yv, div, temp);
212
213 /* Store [y(k) y(k+1) y(k+2) y(k+3)] */
214 vst1q_f32(pDst, yv);
215 pDst+=4;
216
217 blkCnt-=4;
218 }
219 #endif
220 while( *pXq <= x[i+1] && blkCnt > 0 )
221 {
222 x_sc = *pXq++;
223
224 *pDst = y[i]+b[i]*(x_sc-x[i])+c[i]*(x_sc-x[i])*(x_sc-x[i])+d[i]*(x_sc-x[i])*(x_sc-x[i])*(x_sc-x[i]);
225
226 pDst++;
227 blkCnt--;
228 }
229 }
230
231 /* Create output for remaining samples (x>=x(n)) */
232 #ifdef ARM_MATH_NEON
233 /* Compute 4 outputs at a time */
234 blkCnt2 = blkCnt >> 2;
235
236 while(blkCnt2 > 0)
237 {
238 /* Load [xq(k) xq(k+1) xq(k+2) xq(k+3)] */
239 xqv = vld1q_f32(pXq);
240 pXq+=4;
241
242 /* Compute [xq(k)-x(i) xq(k+1)-x(i) xq(k+2)-x(i) xq(k+3)-x(i)] */
243 diff = vsubq_f32(xqv, xiv);
244 temp = diff;
245
246 /* y(i) = a(i) + ... */
247 yv = aiv;
248 /* ... + b(i)*(x-x(i)) + ... */
249 yv = vmlaq_f32(yv, biv, temp);
250 /* ... + c(i)*(x-x(i))^2 + ... */
251 temp = vmulq_f32(temp, diff);
252 yv = vmlaq_f32(yv, civ, temp);
253 /* ... + d(i)*(x-x(i))^3 */
254 temp = vmulq_f32(temp, diff);
255 yv = vmlaq_f32(yv, div, temp);
256
257 /* Store [y(k) y(k+1) y(k+2) y(k+3)] */
258 vst1q_f32(pDst, yv);
259 pDst+=4;
260
261 blkCnt2--;
262 }
263
264 /* Tail */
265 blkCnt2 = blkCnt & 3;
266 #else
267 blkCnt2 = blkCnt;
268 #endif
269
270 while(blkCnt2 > 0)
271 {
272 x_sc = *pXq++;
273
274 *pDst = y[i-1]+b[i-1]*(x_sc-x[i-1])+c[i-1]*(x_sc-x[i-1])*(x_sc-x[i-1])+d[i-1]*(x_sc-x[i-1])*(x_sc-x[i-1])*(x_sc-x[i-1]);
275
276 pDst++;
277 blkCnt2--;
278 }
279 }
280
281 /**
282 @} end of SplineInterpolate group
283 */
284
