Complex cfo(Complex signal, double freq_offset, int cycle)
{
// angle in radians
double angle = freq_offset * (sample_time * 1000 * 2 * PI) * cycle;
Complex rot = {
rel: cos(angle),
img: sin(angle)
};
// rotate signal by angle
return mult_complex(signal, rot);
}
void fft_calc(const int N, const double *x, Complex *X, Complex *P, const int step, const Complex *twids){
int k;
Complex *S = P + N/2;
if (N == 1){
X[0].re = x[0];
X[0].im = 0;
return;
}
fft_calc(N/2, x, S, X,2*step, twids);
fft_calc(N/2, x+step, P, X,2*step, twids);
for (k=0;k<N/2;k++){
P[k] = mult_complex(P[k],twids[k*step]);
X[k] = add_complex(S[k],P[k]);
X[k+N/2] = sub_complex(S[k],P[k]);
}
}
void build_source(double *nodes_reg, double *inreg, long nnod, long nnod_reg, mxArray *omega,
double D, double *source, long reg, mxArray *q) {
mxArray *source_strength, *Gns, *zz;
mxArray *temp_result;
double xi, yi, zi;
long inod, i;
double rq;
double *q_real, *q_img;
double *zz_real, *zz_img, *omega_real, *omega_img, *gns_real, *gns_img;
double *s_strength_real, *s_strength_img, *temp_real, *temp_img, temp_var;
source_strength = mxCreateDoubleMatrix(1, 1, mxCOMPLEX);
assert(source_strength != NULL);
(*(mxGetPr(source_strength))) = 12.4253;
(*(mxGetPi(source_strength))) = 1.8779;
zz = mxCreateDoubleMatrix(1,1,mxCOMPLEX);
Gns = mxCreateDoubleMatrix(1,1,mxCOMPLEX);
temp_result = mxCreateDoubleMatrix(1,1,mxCOMPLEX);
q_real = mxGetPr(q);
q_img = mxGetPi(q);
for(i=0; i<nnod; i++)
*(q_real+i) = *(q_img+i) = 0.0;
zz_real = mxGetPr(zz);
zz_img = mxGetPi(zz);
omega_real = mxGetPr(omega);
omega_img = mxGetPi(omega);
s_strength_real = mxGetPr(source_strength);
s_strength_img = mxGetPi(source_strength);
temp_real = mxGetPr(temp_result);
temp_img = mxGetPi(temp_result);
for(i = 0; i < nnod_reg; ++i) {
inod = (long) inreg[i];
xi = nodes_reg(i,0);
yi = nodes_reg(i,1);
zi = nodes_reg(i,2);
if(reg == 1) {
rq = sqrt(pow(source[0]-xi,2) + pow(source[1]-yi,2)
+ pow(source[2]-zi,2));
*zz_real = (*omega_real) * rq;
*zz_img = (*omega_img) * rq;
temp_var = 4*PI*rq*D;
gns_real = mxGetPr(Gns);
gns_img = mxGetPi(Gns);
cis((*zz_real * -1.0), (*zz_img * -1.0), *gns_real, *gns_img);
(*gns_real) /= temp_var;
(*gns_img) /= temp_var;
mult_complex(*gns_real, *gns_img, *s_strength_real, *s_strength_img,
*temp_real, *temp_img);
(*(q_real+(inod-1))) += *temp_real;
(*(q_img+(inod-1))) += *temp_img;
}
}
mxDestroyArray(zz);
mxDestroyArray(temp_result);
mxDestroyArray(Gns);
mxDestroyArray(source_strength);
}