Zephyr Scientific Library (zscilib)
interp.c
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019 Kevin Townsend (KTOWN)
3  *
4  * SPDX-License-Identifier: Apache-2.0
5  */
6 
7 #include <math.h>
8 #include <errno.h>
9 #include <zephyr/kernel.h>
10 #include <zsl/zsl.h>
11 #include <zsl/interp.h>
12 
13 int
15 {
16  int rc;
17 
18  /* Ensure t = 0.0..1.0 */
19  if ((t < 0.0f) || (t > 1.0f)) {
20  rc = -EINVAL;
21  *v = NAN;
22  goto err;
23  }
24 
25  *v = (1.0f - t) * v0 + t * v1;
26 
27  return 0;
28 err:
29  return rc;
30 }
31 
32 int
33 zsl_interp_find_x(struct zsl_interp_xy xy[], size_t n, zsl_real_t x, int *idx)
34 {
35  int rc;
36  unsigned int idx_upper; /* Upper limit */
37  unsigned int idx_mid; /* Midpoint */
38  unsigned int idx_lower; /* Lower limit */
39  int order; /* Ascending (1) or descending (0) */
40 
41  /* Init lower and upper limits. */
42  idx_lower = 0;
43  idx_upper = n;
44 
45  /* Make sure we have an appropriately large dataset. */
46  if (n < 2) {
47  *idx = -1;
48  rc = -EINVAL;
49  goto err;
50  }
51 
52  /* Determine order (1 = ascending, 0 = descending). */
53  order = (xy[n - 1].x >= xy[0].x);
54 
55  /* xy[0] and xy[n-1] bounds checks. */
56  if ((x > xy[n - 1].x && order) || (x < xy[n - 1].x && !order)) {
57  /* Out of bounds on the high end. */
58  *idx = n;
59  rc = -EINVAL;
60  goto err;
61  } else if ((x < xy[0].x && order) || (x > xy[0].x && !order)) {
62  /* Out of bounds on the low end. */
63  *idx = -1;
64  rc = -EINVAL;
65  goto err;
66  }
67  ;
68 
69  /* Repetitive mid-point computation until a match is made. */
70  while (idx_upper - idx_lower > 1) {
71  idx_mid = (idx_upper + idx_lower) >> 1;
72  if ((x >= xy[idx_mid].x && order) ||
73  (x <= xy[idx_mid].x && !order)) {
74  /* Set lower limit to current mid-point. */
75  idx_lower = idx_mid;
76  } else {
77  /* Set upper limit to current mid-point. */
78  idx_upper = idx_mid;
79  }
80  }
81 
82  /* Set the output index value. */
83  if (x == xy[0].x) {
84  /* Return absolute lower limit. */
85  *idx = 0;
86  } else if (x == xy[n - 1].x) {
87  /* Return absolute upper limit. */
88  *idx = n - 2;
89  } else {
90  /* Return a value in between. */
91  *idx = idx_lower;
92  }
93 
94  return 0;
95 err:
96  return rc;
97 }
98 
99 int
100 zsl_interp_nn(struct zsl_interp_xy *xy1, struct zsl_interp_xy *xy3,
101  zsl_real_t x2, zsl_real_t *y2)
102 {
103  int rc;
104  zsl_real_t delta;
105 
106  /* Make sure there is a delta x between xy1 and xy3. */
107  delta = xy3->x - xy1->x;
108  if (delta < 1E-6F && -delta < 1E-6F) {
109  rc = -EINVAL;
110  *y2 = NAN;
111  goto err;
112  }
113 
114  /* Ensure that x1 <= x2 <= x3. */
115  if ((x2 < xy1->x) || (x2 > xy3->x)) {
116  rc = -EINVAL;
117  goto err;
118  }
119 
120  /* Determine which value is closest, rounding up on 0.5. */
121  *y2 = x2 >= (xy1->x * 1.5f) ? xy3->y : xy1->y;
122 
123  return 0;
124 err:
125  return rc;
126 }
127 
128 int
129 zsl_interp_nn_arr(struct zsl_interp_xy xy[], size_t n,
130  zsl_real_t x, zsl_real_t *y)
131 {
132  int rc;
133  int idx;
134 
135  /* Find the starting position in xy[] for x. */
136  rc = zsl_interp_find_x(xy, n, x, &idx);
137  if (rc) {
138  *y = NAN;
139  goto err;
140  }
141 
142  /* Perform nearest neighbour interp. between xy[idx] and xy[idx+1]. */
143  rc = zsl_interp_nn(&xy[idx], &xy[idx + 1], x, y);
144  if (rc) {
145  *y = NAN;
146  goto err;
147  }
148 
149  return 0;
150 err:
151  return rc;
152 }
153 
154 int
156  zsl_real_t x2, zsl_real_t *y2)
157 {
158  int rc;
159  zsl_real_t delta;
160 
161  /* Make sure there is a delta on x between xy1 and xy3. */
162  delta = xy3->x - xy1->x;
163  if (delta < 1E-6F && -delta < 1E-6F) {
164  rc = -EINVAL;
165  *y2 = NAN;
166  goto err;
167  }
168 
169  /* Ensure that x2 >= x1 && x2 <= x3. */
170  if ((x2 < xy1->x) || (x2 > xy3->x)) {
171  rc = -EINVAL;
172  *y2 = NAN;
173  goto err;
174  }
175 
176  /*
177  * (x2 - x1)(y3 - y1)
178  * y2 = ------------------- + y1
179  * (x3 - x1)
180  */
181 
182  *y2 = ((x2 - xy1->x) * (xy3->y - xy1->y)) / (xy3->x - xy1->x) + xy1->y;
183 
184  return 0;
185 err:
186  return rc;
187 }
188 
189 int
190 zsl_interp_lin_y_arr(struct zsl_interp_xy xy[], size_t n,
191  zsl_real_t x, zsl_real_t *y)
192 {
193  int rc;
194  int idx;
195 
196  /* Find the starting position in xy[] for x. */
197  rc = zsl_interp_find_x(xy, n, x, &idx);
198  if (rc) {
199  *y = NAN;
200  goto err;
201  }
202 
203  /* Perform linear interpolation of x between xy[idx] and xy[idx+1]. */
204  rc = zsl_interp_lin_y(&xy[idx], &xy[idx + 1], x, y);
205  if (rc) {
206  *y = NAN;
207  goto err;
208  }
209 
210  return 0;
211 err:
212  return rc;
213 }
214 
215 int
217  zsl_real_t y2, zsl_real_t *x2)
218 {
219  int rc;
220  zsl_real_t min, max, delta;
221 
222  /* Make sure there is a delta on x between xy1 and xy3. */
223  delta = xy3->x - xy1->x;
224  if (delta < 1E-6F && -delta < 1E-6F) {
225  rc = -EINVAL;
226  *x2 = NAN;
227  goto err;
228  }
229 
230  /* Ensure that y2 is within the range of min and max. */
231  min = xy1->y < xy3->y ? xy1->y : xy3->y;
232  max = xy1->y < xy3->y ? xy3->y : xy1->y;
233  if ((y2 < min) || (y2 > max)) {
234  rc = -EINVAL;
235  *x2 = NAN;
236  goto err;
237  }
238 
239  /*
240  * (y3 - y2) * x1 + (y2 - y1) * x3
241  * x2 = -------------------------------
242  * y3 - y1
243  */
244 
245  *x2 = ((xy3->y - y2) * xy1->x + (y2 - xy1->y) *
246  xy3->x) / (xy3->y - xy1->y);
247 
248  return 0;
249 err:
250  return rc;
251 }
252 
253 int
254 zsl_interp_cubic_calc(struct zsl_interp_xyc xyc[], size_t n, zsl_real_t yp1,
255  zsl_real_t ypn)
256 {
257  int rc;
258  int i;
259  int k;
260  zsl_real_t sigma;
261  zsl_real_t p;
262  zsl_real_t qn;
263  zsl_real_t un;
264  zsl_real_t *u;
265 
266  /* Make sure we have at least three values. */
267  if (n < 3) {
268  rc = -EINVAL;
269  goto err;
270  }
271 
272  u = (zsl_real_t *)k_malloc((n - 1) * sizeof(zsl_real_t));
273  if (u == NULL) {
274  rc = -ENOMEM;
275  goto err;
276  }
277 
278  /* TODO: Remove 'f' restrictions? */
279  if (yp1 > 0.99e30f) {
280  xyc[0].y2 = u[0] = 0.0f;
281  } else {
282  xyc[0].y2 = -0.5f;
283  u[0] = (3.0f / (xyc[1].x - xyc[0].x)) *
284  ((xyc[1].y - xyc[0].y) / (xyc[1].x - xyc[0].x) - yp1);
285  }
286 
287  for (i = 1; i < n - 1; i++) {
288  /* Break out common values. */
289  zsl_real_t x_i_im1 = xyc[i].x - xyc[i - 1].x;
290  zsl_real_t x_ip1_im1 = xyc[i + 1].x - xyc[i - 1].x;
291  sigma = x_i_im1 / x_ip1_im1;
292  p = sigma * xyc[i - 1].y2 + 2.0f;
293  xyc[i].y2 = (sigma - 1.0f) / p;
294  u[i] = (xyc[i + 1].y - xyc[i].y) / (xyc[i + 1].x - xyc[i].x) -
295  (xyc[i].y - xyc[i - 1].y) / (x_i_im1);
296  u[i] = (6.0f * u[i] / (x_ip1_im1) - sigma * u[i - 1]) / p;
297  }
298 
299  if (ypn > 0.99e30f) {
300  qn = un = 0.0f;
301  } else {
302  qn = 0.5f;
303  un = (3.0f / (xyc[n - 1].x - xyc[n - 2].x)) *
304  (ypn - (xyc[n - 1].y -
305  xyc[n - 2].y) / (xyc[n - 1].x - xyc[n - 2].x));
306  }
307 
308  xyc[n - 1].y2 = (un - qn * u[n - 2]) / (qn * xyc[n - 2].y2 + 1.0f);
309 
310  for (k = n - 2; k >= 0; k--) {
311  xyc[k].y2 = xyc[k].y2 * xyc[k + 1].y2 + u[k];
312  }
313 
314  k_free(u);
315 
316  return 0;
317 err:
318  return rc;
319 }
320 
321 int
322 zsl_interp_cubic_arr(struct zsl_interp_xyc xyc[], size_t n,
323  zsl_real_t x, zsl_real_t *y)
324 {
325  int rc;
326  int k; /* Array index value for mid point. */
327  int klo; /* Array index value for low point. */
328  int khi; /* Array index value for high point. */
329  static int pklo; /* Per. low pnt for repeat bisection search. */
330  static int pkhi; /* Per. high pnt for repeat bisection search. */
331  zsl_real_t h; /* xyc[j+1].x - xyc[j].x */
332  zsl_real_t a; /* (xyc[j+1].x - x) / h */
333  zsl_real_t b; /* (x - xyc[j].x) / h */
334 
335  pklo = 0;
336  pkhi = 1;
337 
338  /* Make sure we have at least three values. */
339  if (n < 3) {
340  rc = -EINVAL;
341  goto err;
342  }
343 
344  /* First check if pklo and pkhi from the last run are still a valid
345  * match. The static values are maintained across calls to this
346  * function, allowing us to avoid unnecessarily performing a full array
347  * search. */
348  if (xyc[pklo].x <= x && xyc[pkhi].x > x) {
349  klo = pklo;
350  khi = pkhi;
351  } else {
352  /* Search the full array for x using bisection. */
353  klo = 0;
354  khi = n - 1;
355  while (khi - klo > 1) {
356  /* Set the midpoint based on the current high/low
357  * points.. */
358  k = (khi + klo) >> 1;
359  /* Determine whether we need to search in upper or
360  * lower half. */
361  if (xyc[k].x > x) {
362  /* If the current midpoint is greater than
363  * search value 'x' set the high marker to the
364  * current midpoint, which will cause search to
365  * continue in the lower half. */
366  khi = k;
367  } else {
368  /* Otherwise set the low marker to the current
369  * midpoint, which will cause search to
370  * continue in the upper half. */
371  klo = k;
372  }
373  /* Search until results are reduced to neighbouring xyc
374  * values. */
375  }
376  /* Persist pklo and pkhi for future calls to this function. */
377  pklo = klo;
378  pkhi = khi;
379  }
380 
381  h = xyc[khi].x - xyc[klo].x;
382  if (h == 0) {
383  /* No diff = invalid x input! */
384  rc = -EINVAL;
385  goto err;
386  }
387 
388  /* Calculate coefficients for hi-x (a) and x-lo (b). */
389  a = (xyc[khi].x - x) / h;
390  b = (x - xyc[klo].x) / h;
391 
392  /* Interpolate for y based on a, b using prev. calculated y2 vals. */
393  *y = a * xyc[klo].y + b * xyc[khi].y +
394  ((a * a * a - a) * xyc[klo].y2 + (b * b * b - b) *
395  xyc[khi].y2) * (h * h) / 6.0f;
396 
397  return 0;
398 err:
399  return rc;
400 }
zsl_interp_lin_y
int zsl_interp_lin_y(struct zsl_interp_xy *xy1, struct zsl_interp_xy *xy3, zsl_real_t x2, zsl_real_t *y2)
Linear (AKA 'piecewise linear') interpolation for Y between two points, based on zsl_real_ts.
Definition: interp.c:155
zsl_interp_lin_x
int zsl_interp_lin_x(struct zsl_interp_xy *xy1, struct zsl_interp_xy *xy3, zsl_real_t y2, zsl_real_t *x2)
Linear (AKA 'piecewise linear') interpolation for X between two points, based on zsl_real_ts.
Definition: interp.c:216
zsl_interp_cubic_arr
int zsl_interp_cubic_arr(struct zsl_interp_xyc xyc[], size_t n, zsl_real_t x, zsl_real_t *y)
Natural cubic spline interpolation between two points, based on zsl_real_ts.
Definition: interp.c:322
zsl_interp_nn
int zsl_interp_nn(struct zsl_interp_xy *xy1, struct zsl_interp_xy *xy3, zsl_real_t x2, zsl_real_t *y2)
Nearest neighbour (AKA 'piecewise constant') interpolation based on zsl_real_ts.
Definition: interp.c:100
interp.h
API header file for interpolation functions in zscilib.
zsl_interp_cubic_calc
int zsl_interp_cubic_calc(struct zsl_interp_xyc xyc[], size_t n, zsl_real_t yp1, zsl_real_t ypn)
Calculates xyc[n].y2 for natural cubic spline interpolation, based on the assigned xyc[n]....
Definition: interp.c:254
zsl_interp_xyc
XY struct for cubic spline interpolation.
Definition: interp.h:46
zsl_interp_xyc::y
zsl_real_t y
Definition: interp.h:48
zsl_interp_lerp
int zsl_interp_lerp(zsl_real_t v0, zsl_real_t v1, zsl_real_t t, zsl_real_t *v)
Calculates a number between two numbers using linear interpolation, where 't' is the interpolation fa...
Definition: interp.c:14
zsl_interp_xyc::y2
zsl_real_t y2
Second derivative from the spline.
Definition: interp.h:49
zsl_interp_xy::x
zsl_real_t x
Definition: interp.h:41
zsl_interp_find_x
int zsl_interp_find_x(struct zsl_interp_xy xy[], size_t n, zsl_real_t x, int *idx)
Uses bisection to search the zsl_interp_xy array for the closest array position to 'x',...
Definition: interp.c:33
zsl.h
API header file for zscilib.
zsl_interp_nn_arr
int zsl_interp_nn_arr(struct zsl_interp_xy xy[], size_t n, zsl_real_t x, zsl_real_t *y)
Nearest neighbour (AKA 'piecewise constant') interpolation based on an array of zsl_real_ts.
Definition: interp.c:129
zsl_interp_lin_y_arr
int zsl_interp_lin_y_arr(struct zsl_interp_xy xy[], size_t n, zsl_real_t x, zsl_real_t *y)
Linear (AKA 'piecewise linear') interpolation for Y between two points, based on zsl_real_ts.
Definition: interp.c:190
zsl_real_t
double zsl_real_t
Definition: zsl.h:51
zsl_interp_xy
XY struct for nearest neighbour and linear interpolation.
Definition: interp.h:40
zsl_interp_xyc::x
zsl_real_t x
Definition: interp.h:47
zsl_interp_xy::y
zsl_real_t y
Definition: interp.h:42