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); }