VecDoub MultipoleExpansion::Forces(const VecDoub& x){
double dPdct = 0., dPdp = 0., dPdr=0.;
VecDoub rpt = conv::CartesianToSphericalPolar(x);
double l1; int m_min, m_max, l_skip=1, m_skip=1;
if(flip or triaxial) l_skip = 2;
m_min = -MMAX; m_max = MMAX;
if(axisymmetric){ m_min=0; m_max=0;}
if(triaxial){ m_min=0;m_skip = 2;}
if(rpt[0]<radial_grid[0]) m_min=0.;
for(int m=-m_min;m<m_max+1;m+=m_skip){
double lm[LMAX-m+1],lmt[LMAX-m+1],lmp[LMAX-m+1];
real_spherical_harmonic_2l_1_deriv_array(
cos(rpt[2]),rpt[1],LMAX,m,lm,lmt,lmp);
for(int l=m;l<LMAX+1;l+=l_skip){
if(rpt[0]<radial_grid[0])
l1 = quad_extrapolate<double>(rpt[0],
{radial_grid[0],radial_grid[1],radial_grid[2]},
{Phi_grid[l][m+l][0],
Phi_grid[l][m+l][1],Phi_grid[l][m+l][2]});
// outside grid assume vacuum
else if(rpt[0]>radial_grid[NR-1])
l1 = Phi_grid[l][m+l][NR-1]*pow(radial_grid[NR-1]/rpt[0],l+1);
else l1 = linterp(radial_grid,Phi_grid[l][m+l],rpt[0]);
dPdp+=l1*lmp[l-m];
dPdct+=l1*lmt[l-m];
// for small r linear
if(rpt[0]<radial_grid[0]){
double grad = (dPhi_grid[l][m+l][1]-dPhi_grid[l][m+l][0])/
(radial_grid[1]-radial_grid[0]);
l1 = grad*(rpt[0]-radial_grid[0])+dPhi_grid[l][m+l][0];
}
// outside grid assume vacuum
else if(rpt[0]>radial_grid[NR-1])
l1 = dPhi_grid[l][m+l][NR-1]*pow(radial_grid[NR-1]/rpt[0],l+2);
else l1 = linterp(radial_grid,dPhi_grid[l][m+l],rpt[0]);
dPdr+=-l1*lm[l-m];
// just use l=0, m=0 for central region
if(rpt[0]<radial_grid[0])
break;
}
if(rpt[0]<radial_grid[0])
break;
}
VecDoub f = {0.,0.,0.}; double R2 = x[0]*x[0]+x[1]*x[1];
for(int i=0;i<3;i++) f[i] = dPdr*x[i]/rpt[0];
f[0]+= x[1]/R2*dPdp+x[0]*x[2]/pow(rpt[0],3.)*dPdct;
f[1]+= -x[0]/R2*dPdp+x[1]*x[2]/pow(rpt[0],3.)*dPdct;
f[2]+=-(1.-pow(x[2]/rpt[0],2.))/rpt[0]*dPdct;
return f*(-conv::FPG);
}
double MultipoleExpansion::Phi(const VecDoub& x){
VecDoub rpt = conv::CartesianToSphericalPolar(x);
double p = 0., l1; int m_min, m_max, l_skip=1, m_skip=1;
if(flip or triaxial) l_skip = 2;
m_min = -MMAX; m_max = MMAX;
if(axisymmetric){ m_min=0; m_max=0;}
if(triaxial){ m_min=0;m_skip = 2;}
for(int m=-m_min;m<m_max+1;m+=m_skip){
double lm[LMAX-m+1];
real_spherical_harmonic_2l_1_array(cos(rpt[2]),rpt[1],LMAX,m,lm);
for(int l=m;l<LMAX+1;l+=l_skip){
// outside grid quadratic
if(rpt[0]<radial_grid[0])
l1 = quad_extrapolate<double>(rpt[0],
{radial_grid[0],radial_grid[1],radial_grid[2]},
{Phi_grid[l][m+l][0],
Phi_grid[l][m+l][1],Phi_grid[l][m+l][2]});
// outside grid assume vacuum
else if(rpt[0]>radial_grid[NR-1])
l1 = Phi_grid[l][m+l][NR-1]*pow(radial_grid[NR-1]/rpt[0],l+1);
else l1 = linterp(radial_grid,Phi_grid[l][m+l],rpt[0]);
p+=l1*lm[l-m];
}
}
return -p*conv::FPG;
}
static void find_harmonic_trajectory(llsm_parameters param, FP_TYPE** spectrogram, FP_TYPE** phasegram,
int nfft, int fs, FP_TYPE* f0, int nf0, int idx, FP_TYPE* freq, FP_TYPE* ampl, FP_TYPE* phse) {
const FP_TYPE hardev = 0.3;
FP_TYPE cand_bin[20];
int cand_bin_n = 0;
for(int i = 0; i < nf0; i ++) {
freq[i] = 0;
ampl[i] = 0;
phse[i] = 0;
if(f0[i] < 50 || f0[i] * idx > param.a_mvf)
continue;
int l_idx = round(f0[i] * (idx - hardev) / fs * nfft);
int u_idx = round(f0[i] * (idx + hardev) / fs * nfft);
l_idx = l_idx < 1 ? 1 : l_idx;
u_idx = u_idx > nfft / 2 - 1 ? nfft / 2 - 1 : u_idx;
cand_bin_n = find_peak_cand(cand_bin, spectrogram[i], nfft / 2, l_idx, u_idx, 0.0, 20);
int peak_bin;
if(cand_bin_n > 0)
peak_bin = cand_bin[select_nearest_cand(cand_bin, cand_bin_n, f0[i] * idx / fs * nfft)];
else
peak_bin = find_peak(spectrogram[i], l_idx, u_idx, 1);
FP_TYPE peak_freq, peak_ampl;
interp_peak(& peak_freq, & peak_ampl, spectrogram[i], peak_bin);
freq[i] = peak_freq * fs / nfft;
ampl[i] = exp(peak_ampl);
phse[i] = linterp(phasegram[i][(int)peak_freq], phasegram[i][(int)peak_freq + 1], fmod(peak_freq, 1.0));
if(isnan(ampl[i])) // peak amplitude goes nan if one of the bins = -INF
ampl[i] = 0;
}
}
float yieldCurve::df(int date, InterpolationType it)
{
if (!hasData) throw "Curve is empty";
// return 1 if date is in the past
if (date < todayDate) return 1;
// return 1 if date is in the past
if (date > maxDate) throw "Date is off the curve";
if (it == Default)
{
it = CurvedeDefaultInterpolationType;
}
float df;
switch (it)
{
case Linear:
df = linterp(date, dates.data(), dfs.data());
break;
case LogLinear:
df = loglinterp(date, dates.data(), dfs.data());
break;
default:
throw "Unrecognized interpolation type";
}
return df;
} // float yieldCurve::df(int date, InterpolationType it)
static float linterp2(float x,float t)
{
float ans,st0,st1;
int i,k;
i=SF_MAX(0,SF_MIN(nx1-1,floor(x/hx)));
k=SF_MAX(0,SF_MIN(nt1-1,floor(t/ht)));
if( k<nt1 ) {
st0=linterp1(i,k,x);
st1=linterp1(i,k+1,x);
ans=linterp(st0,st1,ht,t);
}
else ans=linterp1(i,k,x);
return ans;
}
static char solve1pt(int ind,int inda,float ha,char ch)
{
int i,k,k0;
float xa,ta,t,st0,st1;
char ch1='y';
xa=*(x0+inda);
ta=*(t0+inda);
i=floor(xa/hx);
k=floor(ta/ht);
if( k<nt1 ) {
k0=k;
while( ch1=='y' ) {
st0=linterp1(i,k0,xa);
st1=linterp1(i,k0+1,xa);
t=(ta/ha+(st0*(k0+1)-st1*k0))/(1.0/ha-(st1-st0)/ht);
if( t>ta && t<=(k0+1)*ht ) {
ch1='n';
if( *(pup+ind)<=1 ) {
if( t<(*(t0+ind)) ) {
*(pup+ind)=1;
*(t0+ind)=t;
*(x0+ind)=xa;
*(v+ind)=1.0/linterp(st0,st1,ht,t);;
ch='s';
}
ch=(ch=='s') ? 's' : 'n';
}
ch=(ch=='s') ? 's' : 'n';
}
else {
k0++;
if( k0==nt1 ) {
ch1='n';
ch=(ch=='s') ? 's' :'f';
}
}
}
}
else ch=(ch=='s') ? 's' :'f';
return ch;
}
int main(int argc, char *argv[]){
// Number of points to be fitted with a given spline
int dim = 10;
// Number of points for the fitted graph
int steps = 100;
// Allocate memory for a vector x with all the x-coordinates
double *x1 = malloc(dim*sizeof(double));
double *x2 = malloc(dim*sizeof(double));
// Allocate memory for a vector y with all the y-coordinates
double *y1 = malloc(dim*sizeof(double));
double *y2 = malloc(dim*sizeof(double));
// Allocation for the points (z,s) for the interpolant
double z,s;
// Allocation of step variables
double x0, xn, h;
/*
* A. (6 points) Linear and quadratic splines
*/
// Input n points for the interpolation
printf("# x y (points)\n");
for(int i = 0; i < dim; i++){
x1[i] = i + 0.5*sin(i);
y1[i] = i + cos(i*i);
printf(" %2.6f %2.6f \n",x1[i],y1[i]);
}
// Calculate m points for the interpolants
x0 = x1[0];
xn = x1[dim-1];
h = (xn - x0)/steps;
// Linear spline interpolation
s=0;
z=0;
printf("\n\n"); //New block (Gnuplot)
printf("# x y (liner spline interpolation)\n");
for(int i = 0; i < steps; i++){
z = x0 + h*i;
s = linterp(z,x1,y1,dim);
printf(" %2.6f %2.6f \n", z, s);
}
// Quadratic spline interpolation
s=0;
z=0;
qspline *qs = qspline_new(dim,x1,y1);
printf("\n\n"); //New block (Gnuplot)
printf("# x y (quadratic spline interpolation)\n");
for(int i = 0; i < steps; i++){
z = x0 + h*i;
s = qspline_get(qs,z);
printf(" %2.6f %2.6f \n", z, s);
}
/*
* B. (3 points) derivatives and integrals with quadratic spline
*/
//Input for the derivatives and integrals
fprintf(stderr, "# x y (points)\n");
for(int i = 0 ;i<dim; i++){
x2[i] = i*2*PI/(dim-1);
y2[i] = sin(x2[i]);
fprintf(stderr," %2.6f %2.6f \n", x2[i], y2[i]);
}
x0 = x2[0];
xn = x2[dim-1];
h = (xn - x0)/steps;
// Quadratic spline interpolation
s=0;
z=0;
qs = qspline_new(dim,x2,y2);
fprintf(stderr, "\n\n"); //New block (Gnuplot)
fprintf(stderr, "# x y (quadratic spline interpolation)\n");
for(int i = 0; i < steps; i++){
z = x0 + h*i;
s = qspline_get(qs, z);
fprintf(stderr, " %2.6f %2.6f \n", z, s);
}
// Derivative of the quadratic spline interpolation
fprintf(stderr, "\n\n"); //New block (Gnuplot)
fprintf(stderr, "# x y (derivative of the quadratic spline)\n");
for(int i = 0; i < steps; i++){
z = x0 + h*i;
s = qspline_deriv(qs, z);
fprintf(stderr," %2.6f %2.6f \n", z, s);
}
// Integration the quadratic spline interpolation
fprintf(stderr, "\n\n"); //New block (Gnuplot)
fprintf(stderr, "# x y (integration of the quadratic spline)\n");
for(int i = 0; i < steps; i++){
z = x0 + h*i;
s = qspline_int(qs, z);
fprintf(stderr," %2.6f %2.6f \n", z, s);
}
/*
* C. (1 point) Derivatives and integrals with cubic spline
*/
// Cubic spline interpolation with x and y values from A.
s=0;
z=0;
x0 = x1[0];
xn = x1[dim-1];
h = (xn - x0)/steps;
cspline* cs = cspline_new(dim,x1,y1);
fprintf(stdout, "\n\n"); //New block (Gnuplot)
fprintf(stdout, "# x y (cubic spline interpolation)\n");
for(int i = 0; i < steps; i++){
z = x0 + h*i;
s = cspline_get(cs, z);
fprintf(stdout," %2.6f %2.6f \n", z, s);
}
// Derivative of the cubic spline interpolation with x,y values from B.
s=0;
z=0;
x0 = x2[0];
xn = x2[dim-1];
h = (xn - x0)/steps;
cs = cspline_new(dim,x2,y2);
fprintf(stderr, "\n\n"); //New block (Gnuplot)
fprintf(stderr, "# x y (derivative of the cubic spline)\n");
for(int i = 0; i < steps; i++){
z = x0 + h*i;
s = cspline_deriv(cs, z);
fprintf(stderr," %2.6f %2.6f \n", z, s);
}
// Integration of the cubic spline interpolation with x,y values from B.
fprintf(stderr, "\n\n"); //New block (Gnuplot)
fprintf(stderr, "# x y (derivative of the cubic spline)\n");
for(int i = 0; i < steps; i++){
z = x0 + h*i;
s = cspline_int(cs, z);
fprintf(stderr," %2.6f %2.6f \n", z, s);
}
// Free memory
qspline_free(qs);
cspline_free(cs);
free(x1);
free(x2);
free(y1);
free(y2);
fclose(stdout);
fclose(stderr);
return 0;
}
/* Perform interpolation, if necessary, to derive the final
output value for this obsmode from the appropriate row.
*/
double ComputeValue(PhtRow *tabrow, PhotPar *obs) {
/* Parameters:
PhtRow *tabrow: values read from matching row of table
char *obsmode: full obsmode string from header
obsmode string needs to contain values of parameterized values
appropriate for this observation.
*/
double value;
int parnum;
int n,p, nx, ndim;
double *out;
double *obsindx; /* index of each obsmode par value in tabrow->parvals */
double *obsvals; /* value for each obsmode par in the same order as tabrow */
int *ndpos;
int **bounds; /* [ndim,2] array for bounds around obsvals values */
double resvals[2]; /* Values from tabrow->results for interpolation */
double resindx[2]; /* 1-D indices into tabrow->results for bounding positions*/
int posindx; /* N dimensional array of indices for a position */
int indx,pdim,ppos,xdim,xpos;
int tabparlen;
/*
intermediate products used in iterating over dims
*/
int iter, x;
int dimpow,iterpow;
double *ndposd;
int b0,b1,pindx;
int deltadim; /* index of varying dimension */
double bindx[2],bvals[2]; /* indices into results array for bounding values */
double rinterp; /* placeholder for interpolated result */
BoundingPoint **points; /* array of bounding points defining the area of interpolation */
/* Define functions called in this functions */
double linterp(double *, int, double *, double);
void byteconvert(int, int *, int);
int computedeltadim(BoundingPoint *, BoundingPoint *);
long computeindex(int *, double *, int);
void computebounds(double *, int, double, int *, int*);
int strneq_ic(char *, char*, int);
BoundingPoint **InitBoundingPointArray(int, int);
void FreeBoundingPointArray(BoundingPoint **, int);
/* Initialize variables and allocate memory */
value = 0.0;
ndim = tabrow->parnum;
if (ndim == 0) {
/* No parameterized values, so simply return value
stored in 1-element array results */
return(tabrow->results[0]);
}
dimpow = pow(2,ndim);
obsindx = (double *)calloc(ndim, sizeof(double));
obsvals = (double *)calloc(ndim, sizeof(double)); /* Final answer */
ndpos = (int *)calloc(ndim, sizeof(int));
ndposd = (double *)calloc(ndim, sizeof(double));
bounds = (int **) calloc(ndim, sizeof(int *));
for (p=0;p<ndim;p++) bounds[p] = (int *) calloc(2, sizeof(int));
/* We have parameterized values, so linear interpolation
will be needed in all dimensions to get correct value.
Start by getting the floating-point indices for each
parameterized value
*/
/*
Start by matching up the obsmode parameters with those in the table row
These are the values along each dimension of the interpolation
*/
for (p=0;p<ndim;p++){
tabparlen = strlen(tabrow->parnames[p]);
for(n=0;n<obs->npar;n++){
if (strneq_ic(tabrow->parnames[p],obs->parnames[n],tabparlen)){
obsvals[p] = obs->parvalues[n];
break;
}
}
if (obsvals[p] == 0.0) {
printf("ERROR: No obsmode value found for %s\n",tabrow->parnames[p]);
free(obsindx);
free(obsvals);
free(ndpos);
free(ndposd);
for (p=0;p<ndim;p++) free(bounds[p]);
free(bounds);
return ('\0');
}
/* check whether we're going beyond the data in the table (extrapolation) */
/* if we are, return -9999 */
nx = tabrow->nelem[p+1];
if ((obsvals[p] < tabrow->parvals[p][0]) ||
(obsvals[p] > tabrow->parvals[p][nx-1])) {
printf("WARNING: Parameter value %s%f is outside table data bounds.\n",
tabrow->parnames[p],obsvals[p]);
free(obsindx);
free(obsvals);
free(ndpos);
free(ndposd);
for (p=0;p<ndim;p++) free(bounds[p]);
free(bounds);
return -9999.0;
}
}
/* Set up array of BoundingPoint objects to keep track of information needed
for the interpolation
*/
points = InitBoundingPointArray(dimpow,ndim);
/* Now find the index of the obsmode parameterized value
into each parameterized value array
Equivalent to getting positions in each dimension (x,y,...).
*/
for (p=0;p<ndim;p++){
nx = tabrow->nelem[p+1];
out = (double *) calloc(nx, sizeof(double));
for (n=0; n<nx;n++) out[n] = n;
value = linterp(tabrow->parvals[p], nx, out, obsvals[p]);
if (value == -99) {
free(obsindx);
free(obsvals);
free(ndpos);
free(ndposd);
for (p=0;p<ndim;p++) free(bounds[p]);
free(bounds);
free(out);
return('\0');
}
obsindx[p] = value; /* Index into dimension p */
computebounds(out, nx, (double)floor(value), &b0, &b1);
bounds[p][0] = b0;
bounds[p][1] = b1;
/* Free memory so we can use this array for the next variable*/
free(out);
} /* End loop over each parameterized value */
/*
Loop over each axis and perform interpolation to find final result
For each axis, interpolate between all pairs of positions along the same axis
An example with 3 parameters/dimensions for a point with array index (2.2, 4.7, 1.3):
Iteration 1: for all z and y positions , interpolate between pairs in x
(2,4,1)vs(3,4,1), (2,5,1)vs(3,5,1), (2,4,2)vs(3,4,2), and (2,5,2)vs(3,5,2)
Iteration 2: for all z positions, interpolate between pairs from iteration 1 in y
(2.2, 4,1)vs(2.2, 5, 1) and (2.2, 4, 2)vs(2.2, 5, 2)
Iteration 3: interpolate between pairs from iteration 2 in z
(2.2, 4.7, 1) vs (2.2, 4.7, 2) ==> final answer
*/
for (iter=ndim; iter >0; iter--) {
iterpow = pow(2,iter);
for (p=0;p < iterpow;p++){
pdim = floor(p/2);
ppos = p%2;
if (iter == ndim) {
/* Initialize all intermediate products and perform first
set of interpolations over the first dimension
*/
/* Create a bitmask for each dimension for each position */
byteconvert(p,ndpos,ndim);
for (n=0;n<ndim;n++) {
pindx = bounds[n][ndpos[n]];
points[pdim][ppos].index[n] = (double)pindx;
points[pdim][ppos].pos[n] = tabrow->parvals[n][pindx];
}
/* Determine values from tables which correspond to
bounding positions to be interpolated */
indx = computeindex(tabrow->nelem, points[pdim][ppos].index, ndim);
points[pdim][ppos].value = tabrow->results[indx];
} /* End if(iter==ndim) */
if (ppos == 1) {
/* Determine which axis is varying, so we know which
input value from the obsmode string
we need to use for the interpolation */
deltadim = computedeltadim(&points[pdim][0],&points[pdim][1]);
if (deltadim < 0 || deltadim >= ndim) {
printf("ERROR: Deltadim out of range: %i\n",deltadim);
free(obsindx);
free(obsvals);
free (ndpos);
free (ndposd);
for (p=0;p<ndim;p++)free(bounds[p]);
free(bounds);
return('\0');
}
bindx[0] = points[pdim][0].pos[deltadim];
bindx[1] = points[pdim][1].pos[deltadim];
bvals[0] = points[pdim][0].value;
bvals[1] = points[pdim][1].value;
/*Perform interpolation now and record the results */
rinterp = linterp(bindx, 2, bvals,obsvals[deltadim]);
/*
Update intermediate arrays with results in
preparation for the next iteration
*/
if (rinterp == -99) return('\0');
/* Determine where the result of this interpolation should go */
x = floor((p-1)/2);
xdim = floor(x/2);
xpos = x%2;
/* update bpos and bindx for iteration over next dimension
*/
points[xdim][xpos].value = rinterp;
for (n=0;n<ndim;n++) {
points[xdim][xpos].index[n] = points[pdim][0].index[n];
points[xdim][xpos].pos[n] = points[pdim][0].pos[n];
}
points[xdim][xpos].index[deltadim] = obsindx[deltadim];
points[xdim][xpos].pos[deltadim] = obsvals[deltadim];
} /* Finished with this pair of positions (end if(ppos==1)) */
} /* End loop over p, data stored for interpolation in changing dimension */
} /* End loop over axes(iterations), iter, for interpolation */
/* Record result */
value = points[0][0].value;
/* clean up memory allocated within this function */
free(obsindx);
free(obsvals);
free (ndpos);
free (ndposd);
for (p=0;p<tabrow->parnum;p++)free(bounds[p]);
free(bounds);
FreeBoundingPointArray(points,dimpow);
return (value);
}
