1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
|
#define JOIN2(a, b) a ## _ ## b
#define FUNC2(name, stencil) JOIN2(name, stencil)
#define FUNC(name) FUNC2(name, STENCIL)
#define FUZZ 1e-3
void FUNC(interp2d)(const double *src_start, const double *src_step,
const double *src_val,
const int *src_stride, const int *src_len,
int nb_points, const double *x_vals, const double *y_vals,
double *dst)
{
for (int pt = 0; pt < nb_points; pt++) {
const double x = x_vals[pt];
const double y = y_vals[pt];
double fact[2][STENCIL];
double src_x, src_y, ret;
int idx_x, idx_y;
idx_x = (x - src_start[0]) / src_step[0] - (STENCIL / 2 - 1);
if (idx_x < 0)
idx_x = 0;
else if (idx_x > src_len[0] - STENCIL)
idx_x = src_len[0] - STENCIL;
idx_y = (y - src_start[1]) / src_step[1] - (STENCIL / 2 - 1);
if (idx_y < 0)
idx_y = 0;
else if (idx_y > src_len[1] - STENCIL)
idx_y = src_len[1] - STENCIL;
src_x = idx_x * src_step[0] + src_start[0];
src_y = idx_y * src_step[1] + src_start[1];
if (x < src_x - FUZZ || y < src_y - FUZZ ||
x > src_x + STENCIL * src_step[0] + FUZZ ||
y > src_y + STENCIL * src_step[1] + FUZZ) {
ret = NAN;
goto output;
}
for (int i = 0; i < STENCIL; i++) {
double fact_x = 1.0;
double fact_y = 1.0;
for (int j = 0; j < STENCIL; j++) {
if (i == j)
continue;
fact_x *= (x - (src_x + j * src_step[0])) / (src_step[0] * (i - j));
fact_y *= (y - (src_y + j * src_step[1])) / (src_step[1] * (i - j));
}
fact[0][i] = fact_x;
fact[1][i] = fact_y;
}
ret = 0.0;
for (int j = 0; j < STENCIL; j++) {
double val = 0.0;
for (int i = 0; i < STENCIL; i++)
val += fact[0][i] * src_val[(idx_y + j) * src_stride[1] + (idx_x + i) * src_stride[0]];
ret += fact[1][j] * val;
}
output:
dst[pt] = ret;
}
}
#undef FUZZ
#undef FUNC
#undef FUNC2
#undef JOIN2
|
