291 lines
11 KiB
C++
291 lines
11 KiB
C++
|
//
|
||
|
|
// Author: Joshua Cohen
|
||
|
|
// Copyright 2016
|
||
|
|
//
|
||
|
|
// Lots of template functions here because they're MUCH cleaner than writing each one
|
||
|
|
// out individually
|
||
|
|
|
||
|
|
#include <algorithm>
|
||
|
|
#include <cmath>
|
||
|
|
#include <complex>
|
||
|
|
#include <cstdio>
|
||
|
|
#include <cstdlib>
|
||
|
|
#include <vector>
|
||
|
|
#include "UniformInterp.h"
|
||
|
|
using std::complex;
|
||
|
|
using std::max;
|
||
|
|
using std::min;
|
||
|
|
using std::vector;
|
||
|
|
|
||
|
|
// Options:
|
||
|
|
// U-double, V-float
|
||
|
|
// U-complex<double>, V-complex<double>
|
||
|
|
// U-float, V-float
|
||
|
|
template<class U, class V>
|
||
|
|
U UniformInterp::bilinear(double x, double y, vector<vector<V> > &z) {
|
||
|
|
double x1, x2, y1, y2;
|
||
|
|
U q11, q12, q21, q22, ret;
|
||
|
|
|
||
|
|
x1 = floor(x);
|
||
|
|
x2 = ceil(x);
|
||
|
|
y1 = ceil(y);
|
||
|
|
y2 = floor(y);
|
||
|
|
q11 = z[int(y1)-1][int(x1)-1];
|
||
|
|
q12 = z[int(y2)-1][int(x1)-1];
|
||
|
|
q21 = z[int(y1)-1][int(x2)-1];
|
||
|
|
q22 = z[int(y2)-1][int(x2)-1];
|
||
|
|
|
||
|
|
if ((y1 == y2) && (x1 == x2)) {
|
||
|
|
ret = q11;
|
||
|
|
} else if (y1 == y2) {
|
||
|
|
ret = (((x2 - x)/(x2 - x1))*q11) + (((x - x1)/(x2 - x1))*q21);
|
||
|
|
} else if (x1 == x2) {
|
||
|
|
ret = (((y2 - y)/(y2 - y1))*q11) + (((y - y1)/(y2 - y1))*q12);
|
||
|
|
} else {
|
||
|
|
ret = (q11*(x2 - x)*(y2 - y))/((x2 - x1)*(y2 - y1)) +
|
||
|
|
(q21*(x - x1)*(y2 - y))/((x2 - x1)*(y2 - y1)) +
|
||
|
|
(q12*(x2 - x)*(y - y1))/((x2 - x1)*(y2 - y1)) +
|
||
|
|
(q22*(x - x1)*(y - y1))/((x2 - x1)*(y2 - y1));
|
||
|
|
}
|
||
|
|
return ret;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Note that template functions need explicit type-based forward declarations to compile unambiguously
|
||
|
|
template double UniformInterp::bilinear(double,double,vector<vector<float> >&);
|
||
|
|
template complex<double> UniformInterp::bilinear(double,double,vector<vector<complex<double> > >&);
|
||
|
|
template float UniformInterp::bilinear(double,double,vector<vector<float> >&);
|
||
|
|
|
||
|
|
// Options
|
||
|
|
// U-double, V-float
|
||
|
|
// U-complex<double>, V-complex<double>
|
||
|
|
template<class U, class V>
|
||
|
|
U UniformInterp::bicubic(double x, double y, vector<vector<V> > &z) {
|
||
|
|
int x1, x2, y1, y2;
|
||
|
|
vector<U> dzdx(4), dzdy(4), dzdxy(4), zz(4), q(16), cl(16);
|
||
|
|
vector<vector<U> > c(4,vector<U>(4));
|
||
|
|
U qq,ret;
|
||
|
|
double t,u;
|
||
|
|
// Unfortunately there's no other way for this function to work than to have this...
|
||
|
|
double wt_arr[16][16] = {{1.0,0.0,-3.0,2.0,0.0,0.0,0.0,0.0,-3.0,0.0,9.0,-6.0,2.0,0.0,-6.0,4.0},
|
||
|
|
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,3.0,0.0,-9.0,6.0,-2.0,0.0,6.0,-4.0},
|
||
|
|
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,9.0,-6.0,0.0,0.0,-6.0,4.0},
|
||
|
|
{0.0,0.0,3.0,-2.0,0.0,0.0,0.0,0.0,0.0,0.0-9.0,6.0,0.0,0.0,6.0,-4.0},
|
||
|
|
{0.0,0.0,0.0,0.0,1.0,0.0,-3.0,2.0,-2.0,0.0,6.0,-4.0,1.0,0.0,-3.0,2.0},
|
||
|
|
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,0.0,3.0,-2.0,1.0,0.0,-3.0,2.0},
|
||
|
|
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-3.0,2.0,0.0,0.0,3.0,-2.0},
|
||
|
|
{0.0,0.0,0.0,0.0,0.0,0.0,3.0,-2.0,0.0,0.0,-6.0,4.0,0.0,0.0,3.0,-2.0},
|
||
|
|
{0.0,1.0,-2.0,1.0,0.0,0.0,0.0,0.0,0.0,-3.0,6.0,-3.0,0.0,2.0,-4.0,2.0},
|
||
|
|
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,3.0,-6.0,3.0,0.0,-2.0,4.0,-2.0},
|
||
|
|
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-3.0,3.0,0.0,0.0,2.0,-2.0},
|
||
|
|
{0.0,0.0,-1.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,3.0,-3.0,0.0,0.0,-2.0,2.0},
|
||
|
|
{0.0,0.0,0.0,0.0,0.0,1.0,-2.0,1.0,0.0,-2.0,4.0,-2.0,0.0,1.0,-2.0,1.0},
|
||
|
|
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,2.0,-1.0,0.0,1.0,-2.0,1.0},
|
||
|
|
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,-1.0,0.0,0.0,-1.0,1.0},
|
||
|
|
{0.0,0.0,0.0,0.0,0.0,0.0,-1.0,1.0,0.0,0.0,2.0,-2.0,0.0,0.0,-1.0,1.0}};
|
||
|
|
vector<vector<double> > wt(16);
|
||
|
|
for (int i=0; i<16; i++) {
|
||
|
|
vector<double> temp(wt_arr[i],wt_arr[i]+16);
|
||
|
|
wt.push_back(temp);
|
||
|
|
}
|
||
|
|
|
||
|
|
x1 = floor(x) - 1;
|
||
|
|
x2 = ceil(x) - 1;
|
||
|
|
y1 = floor(y) - 1;
|
||
|
|
y2 = ceil(y) - 1;
|
||
|
|
zz[0] = z[y1][x1];
|
||
|
|
zz[3] = z[y2][x1];
|
||
|
|
zz[1] = z[y1][x2];
|
||
|
|
zz[2] = z[y2][x2];
|
||
|
|
dzdx[0] = (z[y1][x1+1] - z[y1][x1-1]) / 2.0;
|
||
|
|
dzdx[1] = (z[y1][x2+1] - z[y1][x2-1]) / 2.0;
|
||
|
|
dzdx[2] = (z[y2][x2+1] - z[y2][x2-1]) / 2.0;
|
||
|
|
dzdx[3] = (z[y2][x1+1] - z[y2][x1-1]) / 2.0;
|
||
|
|
dzdy[0] = (z[y1+1][x1] - z[y1-1][x1]) / 2.0;
|
||
|
|
dzdy[1] = (z[y1+1][x2+1] - z[y1-1][x2]) / 2.0;
|
||
|
|
dzdy[2] = (z[y2+1][x2+1] - z[y2-1][x2]) / 2.0;
|
||
|
|
dzdy[3] = (z[y2+1][x1+1] - z[y2-1][x1]) / 2.0;
|
||
|
|
dzdxy[0] = 0.25 * (z[y1+1][x1+1] - z[y1-1][x1+1] - z[y1+1][x1-1] + z[y1-1][x1-1]);
|
||
|
|
dzdxy[3] = 0.25 * (z[y2+1][x1+1] - z[y2-1][x1+1] - z[y2+1][x1-1] + z[y2-1][x1-1]);
|
||
|
|
dzdxy[1] = 0.25 * (z[y1+1][x2+1] - z[y1-1][x2+1] - z[y1+1][x2-1] + z[y1-1][x2-1]);
|
||
|
|
dzdxy[2] = 0.25 * (z[y2+1][x2+1] - z[y2-1][x2+1] - z[y2+1][x2-1] + z[y2-1][x2-1]);
|
||
|
|
|
||
|
|
for (int i=0; i<4; i++) {
|
||
|
|
q[i] = zz[i];
|
||
|
|
q[i+4] = dzdx[i];
|
||
|
|
q[i+8] = dzdy[i];
|
||
|
|
q[i+12] = dzdxy[i];
|
||
|
|
}
|
||
|
|
for (int i=0; i<16; i++) {
|
||
|
|
qq = 0.0;
|
||
|
|
for (int j=0; j<16; j++) {
|
||
|
|
qq = qq + (wt[i][j] * q[j]);
|
||
|
|
}
|
||
|
|
cl[i] = qq;
|
||
|
|
}
|
||
|
|
for (int i=0; i<4; i++) {
|
||
|
|
for (int j=0; j<4; j++) {
|
||
|
|
c[i][j] = cl[(4*i)+j];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
t = (x - x1);
|
||
|
|
u = (y - y1);
|
||
|
|
ret = 0.0;
|
||
|
|
for (int i=3; i>=0; i--) {
|
||
|
|
ret = (t * ret) + (((((c[i][3] * u) + c[i][2])*u) + c[i][1]) * u) + c[i][0];
|
||
|
|
}
|
||
|
|
return ret;
|
||
|
|
}
|
||
|
|
|
||
|
|
template double UniformInterp::bicubic(double,double,vector<vector<float> >&);
|
||
|
|
template complex<double> UniformInterp::bicubic(double,double,vector<vector<complex<double> > >&);
|
||
|
|
|
||
|
|
void UniformInterp::sinc_coef(double beta, double relfiltlen, int decfactor, double pedestal, int weight, int &intplength, int &filtercoef, vector<double> &filter) {
|
||
|
|
double wgt,s,fct,wgthgt,soff;
|
||
|
|
|
||
|
|
intplength = round(relfiltlen / beta);
|
||
|
|
filtercoef = intplength * decfactor;
|
||
|
|
wgthgt = (1.0 - pedestal / 2.0);
|
||
|
|
soff = (filtercoef - 1.0) / 2.0;
|
||
|
|
|
||
|
|
for (int i=0; i<filtercoef; i++) {
|
||
|
|
wgt = (1.0 - wgthgt) + (wgthgt * cos((M_PI * (i - soff)) / soff));
|
||
|
|
s = (floor(i - soff) * beta) / (1.0 * decfactor);
|
||
|
|
if (s != 0.0) fct = sin(M_PI * s) / (M_PI * s);
|
||
|
|
else fct = 1.0;
|
||
|
|
if (weight == 1) filter[i] = fct * wgt;
|
||
|
|
else filter[i] = fct;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// complex<double> == complex*8 (which was the return type in the original Fortran
|
||
|
|
complex<double> UniformInterp::sinc_eval(vector<complex<double> > &arrin, int nsamp, vector<float> &intarr, int idec, int ilen, int intp, double frp) {
|
||
|
|
complex<double> ret(0.0,0.0);
|
||
|
|
int ifrc;
|
||
|
|
if ((intp >= (ilen-1)) && (intp < nsamp)) {
|
||
|
|
ifrc = min(max(0,int(frp*idec)),idec-1);
|
||
|
|
for (int i=0; i<ilen; i++) {
|
||
|
|
ret = ret + (arrin[intp-i] * double(intarr[i + (ifrc*ilen)]));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return ret;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Options:
|
||
|
|
// U-float, V-float, W-float
|
||
|
|
// U-float, V-double, W-double
|
||
|
|
// U-complex<double>, V-complex<double>, W-float
|
||
|
|
template<class U,class V,class W>
|
||
|
|
U UniformInterp::sinc_eval_2d(vector<vector<V> > &arrin, vector<W> &intarr, int idec, int ilen, int intpx, int intpy, double frpx, double frpy, int xlen, int ylen) {
|
||
|
|
U ret(0.0); // Will initialize ret to 0 regardless of type (complex<double> initializes to (0.0,0.0))
|
||
|
|
int ifracx,ifracy;
|
||
|
|
|
||
|
|
if (((intpx >= (ilen-1)) && (intpx < xlen)) && ((intpy >= (ilen-1)) && (intpy < ylen))) {
|
||
|
|
ifracx = min(max(0,int(frpx*idec)),(idec-1));
|
||
|
|
ifracy = min(max(0,int(frpy*idec)),(idec-1));
|
||
|
|
for (int i=0; i<ilen; i++) {
|
||
|
|
for (int j=0; j<ilen; j++) {
|
||
|
|
ret = ret + (arrin[(intpx-i)][(intpy-j)] * double(intarr[(i+(ifracx*ilen))]) * double(intarr[(j+(ifracy*ilen))]));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return ret;
|
||
|
|
}
|
||
|
|
|
||
|
|
template float UniformInterp::sinc_eval_2d(vector<vector<float> >&,vector<float>&,int,int,int,int,double,double,int,int);
|
||
|
|
template float UniformInterp::sinc_eval_2d(vector<vector<double> >&,vector<double>&,int,int,int,int,double,double,int,int);
|
||
|
|
template complex<double> UniformInterp::sinc_eval_2d(vector<vector<complex<double> > >&,vector<float>&,int,int,int,int,double,double,int,int);
|
||
|
|
|
||
|
|
// Spline-related functions originally were in spline.f, but they make more sense to be here instead of in a separate object
|
||
|
|
float UniformInterp::interp2DSpline(int order, int nx, int ny, vector<vector<float> > &Z, double x, double y) {
|
||
|
|
vector<double> A(order), R(order), Q(order), HC(order);
|
||
|
|
double temp;
|
||
|
|
float ret;
|
||
|
|
int i0,j0,indi,indj;
|
||
|
|
bool lodd;
|
||
|
|
|
||
|
|
if ((order < 3) || (order > 20)) {
|
||
|
|
printf("Error in UniformInterp::interp2DSpline - Spline order must be between 3 and 20\n");
|
||
|
|
printf("(given order was %d)\n",order);
|
||
|
|
exit(1);
|
||
|
|
}
|
||
|
|
lodd = (((order / 2) * 2) != order);
|
||
|
|
if (lodd) {
|
||
|
|
i0 = y - 0.5;
|
||
|
|
j0 = x - 0.5;
|
||
|
|
} else {
|
||
|
|
i0 = y;
|
||
|
|
j0 = x;
|
||
|
|
}
|
||
|
|
i0 = i0 - (order / 2) + 1;
|
||
|
|
j0 = j0 - (order / 2) + 1;
|
||
|
|
|
||
|
|
for (int i=1; i<=order; i++) {
|
||
|
|
indi = min(max((i0+i),1),ny);
|
||
|
|
for (int j=1; j<=order; j++) {
|
||
|
|
indj = min(max((j0+j),1),nx);
|
||
|
|
A[(j-1)] = Z[(indi-1)][(indj-1)];
|
||
|
|
}
|
||
|
|
initSpline(A,order,R,Q);
|
||
|
|
HC[i-1] = spline((x-j0),A,order,R);
|
||
|
|
}
|
||
|
|
initSpline(HC,order,R,Q);
|
||
|
|
temp = spline(y-i0,HC,order,R);
|
||
|
|
ret = float(temp);
|
||
|
|
return ret;
|
||
|
|
}
|
||
|
|
|
||
|
|
void UniformInterp::initSpline(vector<double> &Y, int n, vector<double> &R, vector<double> &Q) {
|
||
|
|
double p;
|
||
|
|
|
||
|
|
Q[0] = 0.0;
|
||
|
|
R[0] = 0.0;
|
||
|
|
for (int i=2; i<n; i++) {
|
||
|
|
p = (Q[i-2] / 2.) + 2.;
|
||
|
|
Q[i-1] = -0.5 / p;
|
||
|
|
R[i-1] = ((3 * (Y[i] - (2 * Y[i-1]) + Y[i-2])) - (R[i-2] / 2)) / p;
|
||
|
|
}
|
||
|
|
R[n-1] = 0.0;
|
||
|
|
for (int i=(n-1); i>=2; i--) R[i-1] = (Q[i-1] * R[i]) + R[i-1];
|
||
|
|
}
|
||
|
|
|
||
|
|
double UniformInterp::spline(double x, vector<double> &Y, int n, vector<double> &R) {
|
||
|
|
double xx,t0,t1,ret;
|
||
|
|
int j;
|
||
|
|
|
||
|
|
if (x < 1.) ret = Y[0] + ((x-1.) * (Y[1] - Y[0] - (R[1] / 6.)));
|
||
|
|
else if (x > n) ret = Y[n-1] + ((x-n) * (Y[n-1] - Y[n-2] + (R[n-2] / 6.)));
|
||
|
|
else {
|
||
|
|
j = ifrac(x);
|
||
|
|
xx = x - j;
|
||
|
|
t0 = Y[j] - Y[j-1] - (R[j-1] / 3.) - (R[j] / 6.);
|
||
|
|
t1 = xx * ((R[j-1] / 2.) + (xx * ((R[j] - R[j-1]) / 6)));
|
||
|
|
ret = Y[j-1] + (xx * (t0 + t1));
|
||
|
|
}
|
||
|
|
return ret;
|
||
|
|
}
|
||
|
|
|
||
|
|
int UniformInterp::ifrac(double r) {
|
||
|
|
int ret;
|
||
|
|
ret = r;
|
||
|
|
if (r >= 0.) return ret;
|
||
|
|
if (r == ret) return ret;
|
||
|
|
ret = ret - 1;
|
||
|
|
return ret;
|
||
|
|
}
|
||
|
|
|
||
|
|
double UniformInterp::quadInterpolate(vector<double> &x, vector<double> &y, double xintp) {
|
||
|
|
vector<double> x1(3), y1(3);
|
||
|
|
double a,b,xin,ret;
|
||
|
|
|
||
|
|
xin = xintp - x[0];
|
||
|
|
for (int i=0; i<3; i++) {
|
||
|
|
x1[i] = x[i] - x[0];
|
||
|
|
y1[i] = y[i] - y[0];
|
||
|
|
}
|
||
|
|
a = ((-y1[1] * x1[2]) + (y1[2] * x1[1])) / ((-x1[2] * x1[1] * x1[1]) + (x1[1] * x1[2] * x1[2]));
|
||
|
|
b = (y1[1] - (a * x1[1] * x1[1])) / x1[1];
|
||
|
|
ret = y[0] + (a * xin * xin) + (b * xin);
|
||
|
|
return ret;
|
||
|
|
}
|
||
|
|
|