void fixrts(float d[], int m)
{
void zroots(fcomplex a[], int m, fcomplex roots[], int polish);
int i,j,polish;
fcomplex a[NMAX],roots[NMAX];
a[m]=ONE;
for (j=m-1;j>=0;j--)
a[j]=Complex(-d[m-j],0.0);
polish=1;
zroots(a,m,roots,polish);
for (j=1;j<=m;j++)
if (Cabs(roots[j]) > 1.0)
roots[j]=Cdiv(ONE,Conjg(roots[j]));
a[0]=Csub(ZERO,roots[1]);
a[1]=ONE;
for (j=2;j<=m;j++) {
a[j]=ONE;
for (i=j;i>=2;i--)
a[i-1]=Csub(a[i-2],Cmul(roots[j],a[i-1]));
a[0]=Csub(ZERO,Cmul(roots[j],a[0]));
}
for (j=0;j<=m-1;j++)
d[m-j] = -a[j].r;
}
void ComplexLUBackSubst(pcomplex **a, int n, int *indx, pcomplex *b)
{
int i, ip, j,
ii = -1;
pcomplex sum;
for (i = 0; i < n; i++) {
ip = indx[i];
sum = b[ip];
b[ip] = b[i];
if (ii >= 0) {
for (j = ii; j <= i - 1; j++)
sum = Csub(sum, Cmul(a[i][j], b[j]));
} else if ((sum.re != 0.0) || (sum.im != 0.0))
ii = i;
b[i] = sum;
}
for (i = n - 1; i >= 0; i--) {
sum = b[i];
for (j = i + 1; j < n; j++)
sum = Csub(sum, Cmul(a[i][j], b[j]));
b[i] = Cdiv(sum, a[i][i]);
}
}
void
correctIQ (CXB sigbuf, IQ iq, BOOLEAN isTX, int subchan)
{
int i;
REAL doit;
if (IQdoit == 0) return;
if (subchan == 0) doit = iq->mu;
else doit = 0;
if(!isTX)
{
// if (subchan == 0) // removed so that sub rx's will get IQ correction
for (i = 0; i < CXBhave (sigbuf); i++)
{
iq->del[iq->index] = CXBdata(sigbuf,i);
iq->y[iq->index] = Cadd(iq->del[iq->index],Cmul(iq->w[0],Conjg(iq->del[iq->index])));
iq->y[iq->index] = Cadd(iq->y[iq->index],Cmul(iq->w[1],Conjg(iq->y[iq->index])));
iq->w[1] = Csub(iq->w[1], Cscl(Cmul(iq->y[iq->index],iq->y[iq->index]), doit)); // this is where the adaption happens
CXBdata(sigbuf,i)=iq->y[iq->index];
iq->index = (iq->index+iq->MASK)&iq->MASK;
}
//fprintf(stderr, "w1 real: %g, w1 imag: %g\n", iq->w[1].re, iq->w[1].im); fflush(stderr);
}
else
{
for (i = 0; i < CXBhave (sigbuf); i++)
{
CXBimag (sigbuf, i) += iq->phase * CXBreal (sigbuf, i);
CXBreal (sigbuf, i) *= iq->gain;
}
}
}
void hypser(fcomplex a, fcomplex b, fcomplex c, fcomplex z, fcomplex *series,
fcomplex *deriv)
{
void nrerror(char error_text[]);
int n;
fcomplex aa,bb,cc,fac,temp;
deriv->r=0.0;
deriv->i=0.0;
fac=Complex(1.0,0.0);
temp=fac;
aa=a;
bb=b;
cc=c;
for (n=1;n<=1000;n++) {
fac=Cmul(fac,Cmul(aa,Cdiv(bb,cc)));
deriv->r+=fac.r;
deriv->i+=fac.i;
fac=Cmul(fac,RCmul(1.0/n,z));
*series=Cadd(temp,fac);
if (series->r == temp.r && series->i == temp.i) return;
temp= *series;
aa=Cadd(aa,ONE);
bb=Cadd(bb,ONE);
cc=Cadd(cc,ONE);
}
nrerror("convergence failure in hypser");
}
static double charact_funct1(double uu)
{
double a,b,rs,rsp,sig,tau,tpf1,tpf2, f10, c0, d0;
dcomplex g,z,w,tp1,tp2,DD,CN,ans,d,expo;
tau=T;
a=k*teta;
rs=rho*sigma;
rsp=rs*uu;
sig=sigma*sigma;
b=k+lambda-rs;
if(uu==0)
{
if(b==0)
{
c0=a*T*T/4.0;
d0=T/2.0;
}
else
{
c0=0.5*a*(exp(-b*T)+b*T - 1.0)/b/b;
d0=0.5*(1.0-exp(-b*T))/b;
}
f10=log(S/K)+(r-divid)*T+c0+d0*v;
return f10;
}
z=Complex(-b,rsp);
z=Cmul(z,z);
w=RCmul(sig,Complex(-uu*uu,uu));
d=Csqrt(Csub(z,w));
tp1=Complex(d.r+b,d.i-rsp);
tp2=Complex(-d.r+b,-d.i-rsp);
g=Cdiv(tp2,tp1);
expo=Cexp(RCmul(-tau,d));
DD=Csub(Complex(1,0),expo);
DD=Cdiv(DD,Csub(Complex(1,0),Cmul(g,expo)));
DD=Cmul(DD,RCmul(1.0/sig,tp2));
CN=Csub(Cmul(g,expo), Complex(1,0));
CN=Cdiv(CN,Csub(g, Complex(1,0) ));
tpf1=a*(tau*tp2.r-2.0*Clog(CN).r)/sig;
tpf2=a*(tau*tp2.i-2.0*Clog(CN).i)/sig;
tpf2+=(r-divid)*uu*tau;
ans=Complex(tpf1+v*DD.r,tpf2+v*DD.i+uu*log(S));
ans=Cmul(Cexp(ans),Cexp(Complex(0,-uu*log(K))));
ans=Cdiv(ans,Complex(0,uu));
return ans.r;
}
void zroots(fcomplex a[], int m, fcomplex roots[], int polish)
{
void zroots(fcomplex a[], int m, fcomplex *x, int *its);
int i, its, j, jj;
fcomplex x, b, c, ad[MAXM];
for (j = 0; j <= m; j++) ad[j] = a[j];
for (j = m; j >= 1; j--) {
x Complex(0.0, 0.0);
laguer(ad, j, &x, &its);
if (fabs(x.i) <= 2.0*EPS*fabs(x.r)) x.i = 0.0;
roots[j] = x;
b = ad[j];
for (jj = j - 1; jj >= 0; jj--) {
c = ad[jj];
ad[jj] = b;
b = Cadd(Cmul(x, b), c);
}
}
if (polish)
for (j = 1; j <= m; j++)
laguer(a, m, &roots[j], &its);
for (j = 2; j <= m; j++) {
x = roots[j];
for (i = j - 1; i >= 1; i--) {
if (rootes.r <= x.r) break;
roots[i + 1] = roots[i];
}
roots[i + 1] = x;
}
}
void
filter_OvSv_par (FiltOvSv pflt)
{
int i, j, m = pflt->fftlen, n = pflt->buflen;
COMPLEX *zfvec = pflt->zfvec,
*zivec = pflt->zivec, *zovec = pflt->zovec, *zrvec = pflt->zrvec;
REAL scl = pflt->scale;
/* input sig -> z */
//fftw_one(pflt->pfwd,(fftw_complex *) zrvec, (fftw_complex *) zivec);
fftwf_execute (pflt->pfwd);
/* convolve in z */
for (i = 0; i < m; i++)
zivec[i] = Cmul (zivec[i], zfvec[i]);
/* z convolved sig -> time output sig */
//fftw_one(pflt->pinv, (fftw_complex *) zivec, (fftw_complex *) zovec);
fftwf_execute (pflt->pinv);
/* scale */
for (i = 0; i < n; i++)
zovec[i].re *= scl, zovec[i].im *= scl;
/* prepare input sig vec for next fill */
for (i = 0, j = n; i < n; i++, j++)
zrvec[i] = zrvec[j];
}
void
filter_OvSv(FiltOvSv pflt) {
int i, m = pflt->fftlen, n = pflt->buflen;
COMPLEX *zfvec = pflt->zfvec,
*zivec = pflt->zivec,
*zovec = pflt->zovec,
*zrvec = pflt->zrvec;
REAL scl = pflt->scale;
/* input sig -> z */
fftwf_execute(pflt->pfwd);
/* convolve in z */
for (i = 0; i < m; i++)
zivec[i] = Cmul(zivec[i], zfvec[i]);
/* z convolved sig -> time output sig */
fftwf_execute(pflt->pinv);
/* prepare input sig vec for next fill */
memcpy((void *)zrvec,(const void *)&zrvec[n],n*sizeof(COMPLEX));
}
void
blms_adapt (BLMS blms)
{
int sigsize = CXBhave (blms->signal);
int sigidx = 0;
// fputs("Inside\n",stderr),fflush(stderr);
do {
int j;
memcpy (blms->delay_line, &blms->delay_line[128], sizeof (COMPLEX) * 128); // do overlap move
memcpy (&blms->delay_line[128], &CXBdata (blms->signal, sigidx), sizeof (COMPLEX) * 128); // copy in new data
fftwf_execute (blms->Xplan); // compute transform of input data
for (j = 0; j < 256; j++) {
blms->Yhat[j] = Cmul (blms->What[j], blms->Xhat[j]); // Filter new signal in freq. domain
blms->Xhat[j] = Conjg (blms->Xhat[j]); // take input data's complex conjugate
}
fftwf_execute (blms->Yplan); //compute output signal transform
for (j = 128; j < 256; j++)
blms->y[j] = Cscl (blms->y[j], BLKSCL);
memset (blms->y, 0, 128 * sizeof (COMPLEX));
for (j = 128; j < 256; j++)
blms->error[j] = Csub (blms->delay_line[j], blms->y[j]); // compute error signal
if (blms->filter_type)
memcpy (&CXBdata (blms->signal, sigidx), &blms->y[128], 128 * sizeof (COMPLEX)); // if noise filter, output y
else
memcpy (&CXBdata (blms->signal, sigidx), &blms->error[128], 128 * sizeof (COMPLEX)); // if notch filter, output error
fftwf_execute (blms->Errhatplan); // compute transform of the error signal
for (j = 0; j < 256; j++)
blms->Errhat[j] = Cmul (blms->Errhat[j], blms->Xhat[j]); // compute cross correlation transform
fftwf_execute (blms->UPDplan); // compute inverse transform of cross correlation transform
for (j = 0; j < 128; j++)
blms->update[j] = Cscl (blms->update[j], BLKSCL);
memset (&blms->update[128], 0, sizeof (COMPLEX) * 128); // zero the last block of the update, so we get
// filter coefficients only at front of buffer
fftwf_execute (blms->Wplan);
for (j = 0; j < 256; j++)
{
blms->What[j] = Cadd (Cscl (blms->What[j], blms->leak_rate), // leak the W away
Cscl (blms->Update[j], blms->adaptation_rate)); // update at adaptation rate
}
sigidx += 128; // move to next block in the signal buffer
} while (sigidx < sigsize); // done?
}
/**
* FFT algorithm to invert a Laplace transform
* @param res a real vector containing the result of the inversion. We know that
* the imaginary part of the inversion vanishes.
* @param f : the Laplace transform to be inverted
* @param T : the time horizon up to which the function is to be recovered
* @param eps : precision required on [0, T]
*/
void pnl_ilap_fft(PnlVect *res, PnlCmplxFunc *f, double T, double eps)
{
PnlVectComplex *fft;
int i, N, size;
double h, time_step, a;
double f_a, omega;
dcomplex mul, fac;
h = M_PI / (2 * T);
a = h * log (1 + 1. / eps) / (M_2PI);
N = MAX (sqrt (exp (a * T) / eps), h / (M_2PI * eps)) ;
N = pow (2., ceil (log (N) / M_LN2) );
time_step = M_2PI / (N * h);
fft = pnl_vect_complex_create (N);
size = floor (T / time_step) + 1;
pnl_vect_resize (res, size);
fac = CIexp (-M_2PI / N);
mul = fac;
f_a = Creal (PNL_EVAL_FUNC (f, Complex (a, 0)));
omega = h;
for (i=0 ; i<N ; i++)
{
pnl_vect_complex_set (fft, i, Cmul(mul,PNL_EVAL_FUNC (f, Complex (a, - omega))));
omega += h;
mul = Cmul(mul,fac);
}
pnl_fft_inplace (fft);
mul = CONE;
for (i=0 ; i<size ; i++)
{
double res_i;
res_i = Creal (Cmul (pnl_vect_complex_get (fft, i), mul));
mul = Cmul (mul, fac);
res_i = (h / M_PI) * exp (a * (i + 1) * time_step) * (res_i + .5 * f_a);
PNL_LET (res, i) = res_i;
}
pnl_vect_complex_free (&fft);
}
void shift (void *API, int ranfft, fcomplex *data, double shift)
{
float arg;
int i, n2;
fcomplex cshift;
n2=ranfft/2;
GMT_FFT_1D (API, (float *)data, ranfft, GMT_FFT_FWD, GMT_FFT_COMPLEX);
for(i=0;i<ranfft;i++){
arg = -2.*PI*shift*i/ranfft;
if(i > n2) arg = -2.*PI*shift*(i-ranfft)/ranfft;
cshift=Cexp(arg);
data[i] = Cmul(cshift,data[i]);
}
GMT_FFT_1D (API, (float *)data, ranfft, GMT_FFT_INV, GMT_FFT_COMPLEX);
}
/**
* Complex Hankel function of the second kind
* multiplied by Cexp( I * z)
*
* @param z a complex number
* @param v a real number, the order of the Bessel function
*
*/
dcomplex pnl_complex_bessel_h2_scaled( double v, dcomplex z )
{
int nz, ierr;
dcomplex cy;
int n = 1, kode = 2, m = 2;
if (v >= 0)
{
pnl_zbesh(CADDR(z), &v, &kode, &m, &n, CADDR(cy), &nz, &ierr);
DO_MTHERR("hankel2e:");
}
else
{
cy = pnl_complex_bessel_h2_scaled (-v, z );
cy = Cmul (cy, CIexp (M_PI * v) );
}
return cy;
}
void hypdrv(float s, float yy[], float dyyds[])
{
fcomplex z,y[3],dyds[3];
y[1]=Complex(yy[1],yy[2]);
y[2]=Complex(yy[3],yy[4]);
z=Cadd(z0,RCmul(s,dz));
dyds[1]=Cmul(y[2],dz);
dyds[2]=Cmul(Csub(Cmul(Cmul(aa,bb),y[1]),Cmul(Csub(cc,
Cmul(Cadd(Cadd(aa,bb),ONE),z)),y[2])),
Cdiv(dz,Cmul(z,Csub(ONE,z))));
dyyds[1]=dyds[1].r;
dyyds[2]=dyds[1].i;
dyyds[3]=dyds[2].r;
dyyds[4]=dyds[2].i;
}
/**
* Hankel function of the second kind
* multiplied by Cexp( I * x)
*
* @param x a real number
* @param v a real number, the order of the Bessel function
*
*/
dcomplex pnl_bessel_h2_scaled( double v, double x )
{
int nz, ierr;
dcomplex cy, z = Complex (x, 0.);
int n = 1;
int kode = 2;
int m = 2;
if (v >= 0)
{
pnl_zbesh(CADDR(z), &v, &kode, &m, &n, CADDR(cy), &nz, &ierr);
DO_MTHERR("hankel2:");
}
else
{
cy = pnl_bessel_h2_scaled (-v, x );
cy = Cmul (cy, CIexp (M_PI * v) );
}
return cy;
}
int main(void)
{
int i,polish=TRUE;
static float d[NP1]=
{0.0,6.0,-15.0,20.0,-15.0,6.0,0.0};
fcomplex zcoef[NP1],zeros[NP1],z1,z2;
/* finding roots of (z-1.0)^6=1.0 */
/* first write roots */
zcoef[NPOLES]=ONE;
for (i=NPOLES-1;i>=0;i--)
zcoef[i] = Complex(-d[NPOLES-i],0.0);
zroots(zcoef,NPOLES,zeros,polish);
printf("Roots of (z-1.0)^6 = 1.0\n");
printf("%24s %27s \n","Root","(z-1.0)^6");
for (i=1;i<=NPOLES;i++) {
z1=Csub(zeros[i],ONE);
z2=Cmul(z1,z1);
z1=Cmul(z1,z2);
z1=Cmul(z1,z1);
printf("%6d %12.6f %12.6f %12.6f %12.6f\n",
i,zeros[i].r,zeros[i].i,z1.r,z1.i);
}
/* now fix them to lie within unit circle */
fixrts(d,NPOLES);
/* check results */
zcoef[NPOLES]=ONE;
for (i=NPOLES-1;i>=0;i--)
zcoef[i] = Complex(-d[NPOLES-i],0.0);
zroots(zcoef,NPOLES,zeros,polish);
printf("\nRoots reflected in unit circle\n");
printf("%24s %27s \n","Root","(z-1.0)^6");
for (i=1;i<=NPOLES;i++) {
z1=Csub(zeros[i],ONE);
z2=Cmul(z1,z1);
z1=Cmul(z1,z2);
z1=Cmul(z1,z1);
printf("%6d %12.6f %12.6f %12.6f %12.6f\n",
i,zeros[i].r,zeros[i].i,z1.r,z1.i);
}
return 0;
}
dcomplex ln_phi_BS(dcomplex u,double t,double sigma)
{
dcomplex psi=RCmul(-sigma*sigma*t*0.5,C_op_apib(Cmul(u,u),u));
//printf( " **> %7.4f +i %7.4f \n",psi.r,psi.i);
return psi;
}
DttSP_EXP void
Audio_Callback2 (float **input, float **output, unsigned int nframes)
{
unsigned int thread;
BOOLEAN b = reset_em;
BOOLEAN return_empty=FALSE;
unsigned int i;
for(thread=0;thread<threadno;thread++)
{
if (top[thread].susp) return_empty = TRUE;
}
if (return_empty)
{
for(thread=0;thread<threadno;thread++)
{
memset (output[2*thread], 0, nframes * sizeof (float));
memset (output[2*thread+1], 0, nframes * sizeof (float));
}
return;
}
if (b)
{
//fprintf(stderr, "reset_em!\n"); fflush(stderr);
//fprintf(stdout,"Audio_Callback2: reset_em = TRUE\n"), fflush(stdout);
reset_system_audio(nframes);
for(thread=0;thread<threadno;thread++) {
memset (output[2*thread], 0, nframes * sizeof (float));
memset (output[2*thread+1], 0, nframes * sizeof (float));
}
return;
}
#if 0
if (diversity.flag) {
// Deal with the transmitter first
if ((ringb_float_read_space (top[1].jack.ring.o.l) >= nframes)
&& (ringb_float_read_space (top[1].jack.ring.o.r) >= nframes))
{
ringb_float_read (top[1].jack.ring.o.l, output[2], nframes);
ringb_float_read (top[1].jack.ring.o.r, output[3], nframes);
}
else
{
// rb pathology
//reset_system_audio(nframes);
for(thread=0;thread<threadno;thread++)
{
memset (output[thread], 0, nframes * sizeof (float));
memset (output[thread], 0, nframes * sizeof (float));
}
return;
}
// input: copy from port to ring
if ((ringb_float_write_space (top[1].jack.ring.i.l) >= nframes)
&& (ringb_float_write_space (top[1].jack.ring.i.r) >= nframes))
{
ringb_float_write (top[1].jack.ring.i.l, input[2], nframes);
ringb_float_write (top[1].jack.ring.i.r, input[3], nframes);
}
else
{
// rb pathology
for(thread=0;thread<threadno;thread++)
{
memset (output[thread], 0, nframes * sizeof (float));
memset (output[thread], 0, nframes * sizeof (float));
}
return;
}
// if enough accumulated in ring, fire dsp
if ((ringb_float_read_space (top[1].jack.ring.i.l) >= top[1].hold.size.frames) &&
(ringb_float_read_space (top[1].jack.ring.i.r) >= top[1].hold.size.frames))
sem_post (&top[1].sync.buf.sem);
//
// Deal with the diversity channel next
//
if ((ringb_float_read_space (top[0].jack.ring.o.l) >= nframes)
&& (ringb_float_read_space (top[0].jack.ring.o.r) >= nframes))
{
/*ringb_float_read (top[thread].jack.auxr.o.l, output[l], nframes);
ringb_float_read (top[thread].jack.auxr.o.r, output[r], nframes);*/
ringb_float_read (top[0].jack.ring.o.l, output[2], nframes);
ringb_float_read (top[0].jack.ring.o.r, output[3], nframes);
}
else
{
// rb pathology
//reset_system_audio(nframes);
for(thread=0;thread<threadno;thread++)
{
memset (output[thread], 0, nframes * sizeof (float));
memset (output[thread], 0, nframes * sizeof (float));
}
return;
}
// Deal with the diversity/phased array channel next
// input: copy from port to ring
if ((ringb_float_write_space (top[0].jack.ring.i.l) >= nframes)
&& (ringb_float_write_space (top[0].jack.ring.i.r) >= nframes) &&
(ringb_float_write_space (top[2].jack.ring.i.l) >= nframes)
&& (ringb_float_write_space (top[2].jack.ring.i.r) >= nframes))
{
REAL *l0 = input[0];
REAL *r0 = input[1];
REAL *l2 = input[4];
REAL *r2 = input[5];
for (i=0;i<nframes;i++) {
COMPLEX A = Cmplx(l0[i],r0[i]);
COMPLEX B = Cmplx(l2[i],r2[i]);
A = Cscl(Cadd(A,Cmul(B,diversity.scalar)),diversity.gain);
ringb_float_write (top[0].jack.ring.i.l, &A.re, 1);
ringb_float_write (top[0].jack.ring.i.r, &A.im, 1);
}
/*ringb_float_write (top[thread].jack.auxr.i.l, input[l], nframes);
ringb_float_write (top[thread].jack.auxr.i.r, input[r], nframes);*/
}
else
{
// rb pathology
//reset_system_audio(nframes);
for(thread=0;thread<threadno;thread++)
{
memset (output[thread], 0, nframes * sizeof (float));
memset (output[thread], 0, nframes * sizeof (float));
}
return;
}
// if enough accumulated in ring, fire dsp
if ((ringb_float_read_space (top[0].jack.ring.i.l) >= top[0].hold.size.frames) &&
(ringb_float_read_space (top[0].jack.ring.i.r) >= top[0].hold.size.frames))
sem_post (&top[0].sync.buf.sem);
//
// Deal with 2nd receiver channel now
//
if ((ringb_float_read_space (top[2].jack.ring.o.l) >= nframes)
&& (ringb_float_read_space (top[2].jack.ring.o.r) >= nframes))
{
/*ringb_float_read (top[thread].jack.auxr.o.l, output[l], nframes);
ringb_float_read (top[thread].jack.auxr.o.r, output[r], nframes);*/
ringb_float_read (top[2].jack.ring.o.l, output[4], nframes);
ringb_float_read (top[2].jack.ring.o.r, output[5], nframes);
}
else
{
// rb pathology
//reset_system_audio(nframes);
for(thread=0;thread<threadno;thread++)
{
memset (output[thread], 0, nframes * sizeof (float));
memset (output[thread], 0, nframes * sizeof (float));
}
return;
}
// input: copy from port to ring
if ((ringb_float_write_space (top[2].jack.ring.i.l) >= nframes)
&& (ringb_float_write_space (top[2].jack.ring.i.r) >= nframes))
{
ringb_float_write (top[2].jack.ring.i.l, input[4], nframes);
ringb_float_write (top[2].jack.ring.i.r, input[5], nframes);
}
else
{
// rb pathology
for(thread=0;thread<threadno;thread++)
{
memset (output[thread], 0, nframes * sizeof (float));
memset (output[thread], 0, nframes * sizeof (float));
}
return;
}
// if enough accumulated in ring, fire dsp
if ((ringb_float_read_space (top[2].jack.ring.i.l) >= top[2].hold.size.frames) &&
(ringb_float_read_space (top[2].jack.ring.i.r) >= top[2].hold.size.frames))
sem_post (&top[2].sync.buf.sem);
} else
#endif
for(thread=0; thread<threadno; thread++)
{
int l=2*thread, r = 2*thread+1;
if ((ringb_float_read_space (top[thread].jack.ring.o.l) >= nframes)
&& (ringb_float_read_space (top[thread].jack.ring.o.r) >= nframes))
{
/*ringb_float_read (top[thread].jack.auxr.o.l, output[l], nframes);
ringb_float_read (top[thread].jack.auxr.o.r, output[r], nframes);*/
ringb_float_read (top[thread].jack.ring.o.l, output[l], nframes);
ringb_float_read (top[thread].jack.ring.o.r, output[r], nframes);
}
else
{
// rb pathology
//reset_system_audio(nframes);
for(thread=0;thread<threadno;thread++)
{
memset (output[2*thread ], 0, nframes * sizeof (float));
memset (output[2*thread+1], 0, nframes * sizeof (float));
}
return;
}
// input: copy from port to ring
if ((ringb_float_write_space (top[thread].jack.ring.i.l) >= nframes)
&& (ringb_float_write_space (top[thread].jack.ring.i.r) >= nframes))
{
if (diversity.flag && (thread == 0)) {
if ((ringb_float_write_space (top[2].jack.ring.i.l) >= nframes)
&& (ringb_float_write_space (top[2].jack.ring.i.r) >= nframes))
{
REAL *l0 = input[0];
REAL *r0 = input[1];
REAL *l2 = input[4];
REAL *r2 = input[5];
for (i=0;i<nframes;i++) {
COMPLEX A = Cmplx(l0[i],r0[i]);
COMPLEX B = Cmplx(l2[i],r2[i]);
A = Cscl(Cadd(A,Cmul(B,diversity.scalar)),diversity.gain);
ringb_float_write (top[0].jack.ring.i.l, &A.re, 1);
ringb_float_write (top[0].jack.ring.i.r, &A.im, 1);
}
/*ringb_float_write (top[thread].jack.auxr.i.l, input[l], nframes);
ringb_float_write (top[thread].jack.auxr.i.r, input[r], nframes);*/
} else {
// rb pathology
//reset_system_audio(nframes);
for(thread=0;thread<threadno;thread++)
{
memset (output[2*thread ], 0, nframes * sizeof (float));
memset (output[2*thread+1], 0, nframes * sizeof (float));
}
return;
}
} else {
ringb_float_write (top[thread].jack.ring.i.l, input[l], nframes);
ringb_float_write (top[thread].jack.ring.i.r, input[r], nframes);
/*ringb_float_write (top[thread].jack.auxr.i.l, input[l], nframes);
ringb_float_write (top[thread].jack.auxr.i.r, input[r], nframes);*/
}
}
else
{
// rb pathology
//reset_system_audio(nframes);
for(thread=0;thread<threadno;thread++)
{
memset (output[2*thread ], 0, nframes * sizeof (float));
memset (output[2*thread+1], 0, nframes * sizeof (float));
}
return;
}
// if enough accumulated in ring, fire dsp
if ((ringb_float_read_space (top[thread].jack.ring.i.l) >= top[thread].hold.size.frames) &&
(ringb_float_read_space (top[thread].jack.ring.i.r) >= top[thread].hold.size.frames))
sem_post (&top[thread].sync.buf.sem);
}
}
void cisi(float x, float *ci, float *si)
{
void nrerror(char error_text[]);
int i,k,odd;
float a,err,fact,sign,sum,sumc,sums,t,term;
fcomplex h,b,c,d,del;
t=fabs(x);
if (t == 0.0) {
*si=0.0;
*ci = -1.0/FPMIN;
return;
}
if (t > TMIN) {
b=Complex(1.0,t);
c=Complex(1.0/FPMIN,0.0);
d=h=Cdiv(ONE,b);
for (i=2;i<=MAXIT;i++) {
a = -(i-1)*(i-1);
b=Cadd(b,Complex(2.0,0.0));
d=Cdiv(ONE,Cadd(RCmul(a,d),b));
c=Cadd(b,Cdiv(Complex(a,0.0),c));
del=Cmul(c,d);
h=Cmul(h,del);
if (fabs(del.r-1.0)+fabs(del.i) < EPS) break;
}
if (i > MAXIT) nrerror("cf failed in cisi");
h=Cmul(Complex(cos(t),-sin(t)),h);
*ci = -h.r;
*si=PIBY2+h.i;
} else {
if (t < sqrt(FPMIN)) {
sumc=0.0;
sums=t;
} else {
sum=sums=sumc=0.0;
sign=fact=1.0;
odd=TRUE;
for (k=1;k<=MAXIT;k++) {
fact *= t/k;
term=fact/k;
sum += sign*term;
err=term/fabs(sum);
if (odd) {
sign = -sign;
sums=sum;
sum=sumc;
} else {
sumc=sum;
sum=sums;
}
if (err < EPS) break;
odd=!odd;
}
if (k > MAXIT) nrerror("maxits exceeded in cisi");
}
*si=sums;
*ci=sumc+log(t)+EULER;
}
if (x < 0.0) *si = -(*si);
}
int ComplexLUDecompose(pcomplex **a, int n, double *vv, int *indx, double *pd)
// pcomplex **a; the matrix whose LU-decomposition is wanted
// int n; order of a
// double *vv; work vector of size n (stores implicit
// scaling of each row)
// int *indx; => row permutation according to partial
// pivoting sequence
// double *pd; => 1 if number of row interchanges was even,
// -1 if odd (NULL OK)
{
int i, imax, j, k;
double big, dum, temp, d;
pcomplex sum, cdum;
d = 1.0;
imax = 0; // only to shut the compiler up.
for (i = 0; i < n; i++) {
big = 0.0;
for (j = 0; j < n; j++) {
if ((temp = Cabs(a[i][j])) > big)
big = temp;
}
if (big == 0.0) {
printf("singular matrix in routine ComplexLUDecompose\n");
return 1;
}
vv[i] = 1.0 / big;
}
for (j = 0; j < n; j++) {
for (i = 0; i < j; i++) {
sum = a[i][j];
for (k = 0; k < i; k++)
sum = Csub(sum, Cmul(a[i][k], a[k][j]));
a[i][j] = sum;
}
big = 0.0;
for (i = j; i < n; i++) {
sum = a[i][j];
for (k = 0; k < j; k++)
sum = Csub(sum, Cmul(a[i][k], a[k][j]));
a[i][j] = sum;
dum = vv[i] * Cabs(sum);
if (dum >= big) {
big = dum;
imax = i;
}
}
if (j != imax) {
for (k = 0; k < n; k++) {
cdum = a[imax][k];
a[imax][k] = a[j][k];
a[j][k] = cdum;
}
d = -d;
vv[imax] = vv[j];
}
indx[j] = imax;
if (a[j][j].re == 0.0 && a[j][j].im == 0.0)
a[j][j] = Complex(1.0e-20, 1.0e-20);
if (j != n - 1){
cdum = Cdiv(Complex(1.0, 0.0), a[j][j]);
for (i = j + 1; i < n; i++)
a[i][j] = Cmul(a[i][j], cdum);
}
}
if (pd != NULL)
*pd = d;
return 0;
}
void frenel(float x, float *s, float *c)
{
void nrerror(char error_text[]);
int k,n,odd;
float a,ax,fact,pix2,sign,sum,sumc,sums,term,test;
fcomplex b,cc,d,h,del,cs;
ax=fabs(x);
if (ax < sqrt(FPMIN)) {
*s=0.0;
*c=ax;
} else if (ax <= XMIN) {
sum=sums=0.0;
sumc=ax;
sign=1.0;
fact=PIBY2*ax*ax;
odd=TRUE;
term=ax;
n=3;
for (k=1;k<=MAXIT;k++) {
term *= fact/k;
sum += sign*term/n;
test=fabs(sum)*EPS;
if (odd) {
sign = -sign;
sums=sum;
sum=sumc;
} else {
sumc=sum;
sum=sums;
}
if (term < test) break;
odd=!odd;
n += 2;
}
if (k > MAXIT) nrerror("series failed in frenel");
*s=sums;
*c=sumc;
} else {
pix2=PI*ax*ax;
b=Complex(1.0,-pix2);
cc=Complex(1.0/FPMIN,0.0);
d=h=Cdiv(ONE,b);
n = -1;
for (k=2;k<=MAXIT;k++) {
n += 2;
a = -n*(n+1);
b=Cadd(b,Complex(4.0,0.0));
d=Cdiv(ONE,Cadd(RCmul(a,d),b));
cc=Cadd(b,Cdiv(Complex(a,0.0),cc));
del=Cmul(cc,d);
h=Cmul(h,del);
if (fabs(del.r-1.0)+fabs(del.i) < EPS) break;
}
if (k > MAXIT) nrerror("cf failed in frenel");
h=Cmul(Complex(ax,-ax),h);
cs=Cmul(Complex(0.5,0.5),
Csub(ONE,Cmul(Complex(cos(0.5*pix2),sin(0.5*pix2)),h)));
*c=cs.r;
*s=cs.i;
}
if (x < 0.0) {
*c = -(*c);
*s = -(*s);
}
}
static int compute_price(double tt, double H, double K, double r_premia, double v0, double kappa, double theta, double sigma, double rho,
double L, int M, int Nt )
{
/*Variables*/
int j, n, k;
double r; /*continuous rate*/
double min_log_price, max_log_price;
double ds, dt; /*price and time discretization steps*/
double rho_hat; /*parameter after substitution*/
double q, factor, discount_factor; /*pde parameters*/
double treshold = 1e-9; /* when we assume probability to be zero and switch to a different equation*/
int k_d, k_u; /*n+1 vertice numbers, depending on [n][k]*/
double sigma_local, gamma; /*wh factors parameters*/
double beta_minus, beta_plus; /*wh-factors coefficients*/
double local_barrier; /*a barrier depending on [n][k], to check crossing on each step*/
//if (2.0 * kappa * theta < pow(sigma, 2))
// return 1; /*Novikov condition not satisfied, probability values could be incorrect*/
/*Body*/
r = log(1 + r_premia / 100);
/*building voltree*/
tree_v(tt, v0, kappa, theta, sigma, Nt);
/*spacial variable. Price space construction*/
min_log_price = L*log(0.5) - (rho / sigma)* V[Nt][Nt];
max_log_price = L*log(2);
ds = (max_log_price - min_log_price) / double(M);
for (j = 0; j < M; j++)
{
ba_log_prices[j] = min_log_price + j*ds;
ba_prices[j] = H*exp(ba_log_prices[j] + (rho / sigma)* V[0][0]);
}
dt = tt / double(Nt);
/*fft frequences we'll need in every vertice of a tree*/
fftfreq(M, ds);
rho_hat = sqrt(1.0 - pow(rho, 2.0));
q = 1.0 / dt + r;
factor = pow(q*dt, -1.0);
//discount_factor = exp(r*dt);
discount_factor = r - rho / sigma * kappa * theta;
/*filling F_next matrice by initial (in time T) conditions*/
for (j = 0; j < M; j++)
for (k = 0; k < Nt + 1; k++)
{
F_next[j][k] = Complex(G(H*exp(ba_log_prices[j] + (rho / sigma)* V[Nt][k]), K), 0);
}
/*here the main cycle starts - the backward induction procedure*/
for (n = Nt - 1; n >= 0; n--)
{
printf("Processing: %d of %d\n", n, Nt-1);
for (k = 0; k <= n; k++)
{
/*to calculate the binomial expectation we should use matrices from the tree method.
After (n,k) vertice one could either get to (n+1,k_u) or (n+1, k_d). The numbers k_u and k_d could be
read from f_up and f_down matrices, by the rule of addition, for example:
f_down[i][j] = -z;
Rd = V[i + 1][j - z]
f_up[i][j] = z;
Ru = V[i + 1][j + z];
*/
k_u = k + f_up[n][k];
k_d = k + f_down[n][k];
local_barrier = - (rho / sigma) * V[n][k];
/*initial conditions of a step*/
for (j = 0; j < M; j++)
{
//f_n_plus_1_k_u[j] = F[j][n+1][k_u];
//f_n_plus_1_k_d[j] = F[j][n+1][k_d];
f_n_plus_1_k_u[j] = F_next[j][k_u];
f_n_plus_1_k_d[j] = F_next[j][k_d];
}
/*applying indicator function*/
for (j = 0; j < M; j++)
{
if (ba_log_prices[j] < local_barrier)
{
f_n_plus_1_k_u[j].r = 0.0;
f_n_plus_1_k_u[j].i = 0.0;
f_n_plus_1_k_d[j].r = 0.0;
f_n_plus_1_k_d[j].i = 0.0;
}
}
if (V[n][k] >= treshold)
{
/*set up variance - dependent parameters for a given step*/
sigma_local = rho_hat * sqrt(V[n][k]);
gamma = r - 0.5 * V[n][k] - rho / sigma * kappa * (theta - V[n][k]); /*also local*/
/* beta_plus and beta_minus*/
/*beta_minus = -(gamma + sqrt(gamma^2 + 2 * sigma^2 * q)) / sigma^2
beta_plus = -(gamma - sqrt(gamma^2 + 2 * sigma^2 * q)) / sigma^2*/
beta_minus = -(gamma + sqrt(pow(gamma,2) + 2 * pow(sigma_local,2) * q)) / pow(sigma_local,2);
beta_plus = -(gamma - sqrt(pow(gamma,2) + 2 * pow(sigma_local,2) * q)) / pow(sigma_local,2);
for (j = 0; j < M; j++)
{
/* factor functions
phi_plus_array = ([beta_plus / (beta_plus - i * 2 * pi*xi) for xi in xi_space])
phi_minus_array = ([-beta_minus / (-beta_minus + i * 2 * pi*xi) for xi in xi_space]) */
phi_plus_array[j] = RCdiv(beta_plus, RCsub(beta_plus, RCmul((2.0 * PI * fftfreqs[j]), CI)));
phi_minus_array[j] = RCdiv(-beta_minus, RCadd(-beta_minus, RCmul((2.0 * PI * fftfreqs[j]), CI)));
}
/*factorization calculation*/
/*f_n_k_u = factor * fft.ifft(phi_minus_array * fft.fft(
indicator(original_prices_array, 0) * fft.ifft(phi_plus_array * fft.fft(f_n_plus_1_k_u))))*/
for (int j = 0; j < M; j++)
{
f_n_plus_1_k_u_re[j] = f_n_plus_1_k_u[j].r;
f_n_plus_1_k_u_im[j] = f_n_plus_1_k_u[j].i;
}
pnl_fft2(f_n_plus_1_k_u_re, f_n_plus_1_k_u_im, M);
for (j = 0; j < M; j++) {
/*putting complex and imaginary part together again*/
f_n_plus_1_k_u_fft_results[j] = Complex(f_n_plus_1_k_u_re[j], f_n_plus_1_k_u_im[j]);
/*multiplying by phi_plus*/
f_n_plus_1_k_u_fft_results[j] = Cmul(phi_plus_array[j], f_n_plus_1_k_u_fft_results[j]);
/*extracting imaginary and complex parts to use in further fft*/
f_n_plus_1_k_u_fft_results_re[j] = f_n_plus_1_k_u_fft_results[j].r;
f_n_plus_1_k_u_fft_results_im[j] = f_n_plus_1_k_u_fft_results[j].i;
}
pnl_ifft2(f_n_plus_1_k_u_fft_results_re, f_n_plus_1_k_u_fft_results_im, M);
/*applying indicator function, after ifft*/
for (j = 0; j < M; j++)
{
if (ba_log_prices[j] < local_barrier)
{
f_n_plus_1_k_u_fft_results_re[j] = 0.0;
f_n_plus_1_k_u_fft_results_im[j] = 0.0;
}
}
/*performing second fft */
pnl_fft2(f_n_plus_1_k_u_fft_results_re, f_n_plus_1_k_u_fft_results_im, M);
for (j = 0; j < M; j++) {
/*putting complex and imaginary part together again*/
f_n_plus_1_k_u_fft_results[j] = Complex(f_n_plus_1_k_u_fft_results_re[j], f_n_plus_1_k_u_fft_results_im[j]);
/*multiplying by phi_minus*/
f_n_plus_1_k_u_fft_results[j] = Cmul(phi_minus_array[j], f_n_plus_1_k_u_fft_results[j]);
/*extracting imaginary and complex parts to use in further fft*/
f_n_plus_1_k_u_fft_results_re[j] = f_n_plus_1_k_u_fft_results[j].r;
f_n_plus_1_k_u_fft_results_im[j] = f_n_plus_1_k_u_fft_results[j].i;
}
/*the very last ifft*/
pnl_ifft2(f_n_plus_1_k_u_fft_results_re, f_n_plus_1_k_u_fft_results_im, M);
/*multiplying by factor*/
for (j = 0; j < M; j++) {
f_n_k_u[j].r = factor * f_n_plus_1_k_u_fft_results_re[j];
f_n_k_u[j].i = factor * f_n_plus_1_k_u_fft_results_im[j];
}
/*f_n_k_d = factor * fft.ifft(phi_minus_array * fft.fft(
indicator(original_prices_array, 0) * fft.ifft(phi_plus_array * fft.fft(f_n_plus_1_k_d))))*/
for (int j = 0; j < M; j++)
{
f_n_plus_1_k_d_re[j] = f_n_plus_1_k_d[j].r;
f_n_plus_1_k_d_im[j] = f_n_plus_1_k_d[j].i;
}
pnl_fft2(f_n_plus_1_k_d_re, f_n_plus_1_k_d_im, M);
for (j = 0; j < M; j++) {
/*putting complex and imaginary part together again*/
f_n_plus_1_k_d_fft_results[j] = Complex(f_n_plus_1_k_d_re[j], f_n_plus_1_k_d_im[j]);
/*multiplying by phi_plus*/
f_n_plus_1_k_d_fft_results[j] = Cmul(phi_plus_array[j], f_n_plus_1_k_d_fft_results[j]);
/*extracting imaginary and complex parts to use in further fft*/
f_n_plus_1_k_d_fft_results_re[j] = f_n_plus_1_k_d_fft_results[j].r;
f_n_plus_1_k_d_fft_results_im[j] = f_n_plus_1_k_d_fft_results[j].i;
}
pnl_ifft2(f_n_plus_1_k_d_fft_results_re, f_n_plus_1_k_d_fft_results_im, M);
/*applying indicator function, after ifft*/
for (j = 0; j < M; j++)
{
if (ba_log_prices[j] < local_barrier)
{
f_n_plus_1_k_d_fft_results_re[j] = 0.0;
f_n_plus_1_k_d_fft_results_im[j] = 0.0;
}
}
/*performing second fft */
pnl_fft2(f_n_plus_1_k_d_fft_results_re, f_n_plus_1_k_d_fft_results_im, M);
for (j = 0; j < M; j++) {
/*putting complex and imaginary part together again*/
f_n_plus_1_k_d_fft_results[j] = Complex(f_n_plus_1_k_d_fft_results_re[j], f_n_plus_1_k_d_fft_results_im[j]);
/*multiplying by phi_minus*/
f_n_plus_1_k_d_fft_results[j] = Cmul(phi_minus_array[j], f_n_plus_1_k_d_fft_results[j]);
/*extracting imaginary and complex parts to use in further fft*/
f_n_plus_1_k_d_fft_results_re[j] = f_n_plus_1_k_d_fft_results[j].r;
f_n_plus_1_k_d_fft_results_im[j] = f_n_plus_1_k_d_fft_results[j].i;
}
/*the very last ifft*/
pnl_ifft2(f_n_plus_1_k_d_fft_results_re, f_n_plus_1_k_d_fft_results_im, M);
/*multiplying by factor*/
for (j = 0; j < M; j++) {
f_n_k_d[j].r = factor * f_n_plus_1_k_d_fft_results_re[j];
f_n_k_d[j].i = factor * f_n_plus_1_k_d_fft_results_im[j];
}
}
else if (V[n][k] < treshold)
{
/*applying indicator function*/
for (j = 0; j < M; j++)
{
if (ba_log_prices[j] < local_barrier)
{
f_n_plus_1_k_u[j].r = 0.0;
f_n_plus_1_k_u[j].i = 0.0;
f_n_plus_1_k_d[j].r = 0.0;
f_n_plus_1_k_d[j].i = 0.0;
}
}
for (j = 0; j < M; j++)
{
//f_n_plus_1_k_u[j] = F[j][n + 1][k_u];
f_n_plus_1_k_u[j] = F_next[j][k_u];
f_n_k_u[j] = CRsub(f_n_plus_1_k_u[j], discount_factor * dt);
f_n_k_d[j] = f_n_k_u[j];
}
}
/*
f_n_k = pd_f[n, k] * f_n_k_d + pu_f[n, k] * f_n_k_u
*/
for (j = 0; j < M; j++)
{
f_n_k[j] = Cadd(RCmul(pd_f[n][k], f_n_k_d[j]), RCmul(pu_f[n][k], f_n_k_u[j]));
F_prev[j][k] = f_n_k[j];
}
}
for (j = 0; j < M; j++)
{
for (int state = 0; state < Nt; state++)
{
F_next[j][state] = F_prev[j][state];
F_prev[j][state] = Complex(0,0);
}
}
}
/*Preprocessing F before showing out*/
for (j = 0; j < M; j++)
{
if (ba_prices[j] <= H)
{
F_next[j][0].r = 0;
}
if (F_next[j][0].r < 0.)
{
F_next[j][0].r = 0;
}
}
return OK;
}
static double charact_func(double k)
{
double X,tau,roeps,u,b,I,eps,eps2;
dcomplex Ak,Bk,Ck,Dk,Lambdak,z1,z2,z3,zeta,psi_moins,psi_plus,expo,ans;
dcomplex dlk;
tau = T;
eps = sigma;
roeps = rho*eps;
X = log(S/K) + (r - divid)*tau;
eps2 = eps*eps;
if(func_type==1)
{
u = 1.;
b = kappa - roeps;
I = 1.;
}
else if(func_type==2)
{
u = -1.;
b = kappa;
I = 0.;
}
else
{
printf("erreur : dans charact_func il faut initialiser func_type a 1 ou 2.\n");
exit(-1);
}
if(heston==1)
{
z1 = Complex(k*k,-u*k);
z2 = Complex(b,-roeps*k);
z2 = Cmul(z2,z2);
zeta = Cadd(z2,RCmul(eps2,z1));
zeta = Csqrt(zeta);
psi_moins = Complex(b,-roeps*k);
psi_plus = RCmul(-1.,psi_moins);
psi_moins = Cadd(psi_moins,zeta);
psi_plus = Cadd(psi_plus,zeta);
expo = Cexp( RCmul(-tau,zeta) );
z3 = Cadd( psi_moins , Cmul(psi_plus,expo) );
Bk = RCmul(-1.,z1);
Bk = Cmul( Bk , Csub(Complex(1.,0),expo) );
Bk = Cdiv(Bk,z3);
Ak = Cdiv( z3 , RCmul(2.,zeta) );
Ak = Clog(Ak);
if(initlog>0)
{
dlk = Csub(Ak,lk_1);
if(dlk.i < -M_PI)
{
bk = bk + 1;
}
else if(dlk.i > M_PI)
{
bk = bk - 1;
}
initlog++;
lk_1 = Ak;
} else {
initlog++;
lk_1 = Ak;
}
Ak = Cadd(Ak, Complex(0.,2*M_PI*bk));
Ak = RCmul( 2. , Ak );
Ak = Cadd( RCmul(tau,psi_plus) , Ak);
Ak = RCmul( -kappa*teta/eps2 , Ak);
}
else
{
Ak = Complex(0.,0.);
Bk = Complex( -0.5*tau*k*k , 0.5*tau*u*k );
}
if(merton==1)
{
z1 = Complex( -0.5*v*v*k*k + I*(m0+0.5*v*v) , (m0+I*v*v)*k );
z1 = Cexp(z1);
z2 = Complex(I,k);
z2 = RCmul( exp(m0+0.5*v*v) -1, z2);
z2 = Cadd( Complex(1.,0.) , z2 );
Lambdak = Csub(z1,z2);
Ck = Complex(0.,0.);
Dk = RCmul(tau,Lambdak);
}
else
{
Ck = Complex(0.,0.);
Dk = Complex(0.,0.);
}
ans = Cadd( Ak , RCmul(V0,Bk) );
ans = Cadd( ans , Ck );
ans = Cadd( ans , RCmul(lambda0,Dk) );
ans = Cadd( ans , Complex(0.,k*X) );
ans = Cexp(ans);
ans = Cdiv(ans,Complex(0.,k));
return ans.r;
}
/*getPeak:
This function computes a correlation peak, with SNR, between
the two given images at the given points.
*/
void getPeak(int x1,int y1,char *szImg1,int x2,int y2,char *szImg2,
float *peakX,float *peakY, float *snr)
{
static float *peaks;
static complexFloat *s=NULL, *t, *product;
int peakMaxX, peakMaxY, x,y,xOffset,yOffset,count;
int xOffsetStart, yOffsetStart, xOffsetEnd, yOffsetEnd;
float dx,dy,accel1 = (float)(trgSize/2 - srcSize/2);
float peakMax, thisMax, peakSum;
/*
* Calculate the limits of the time domain correlations...
* A coordinate in the target may be set to:
* ( (trgSize/2 - srcSize/2), (trgSize/2 - srcSize/2) ).
* If this is the ulh element of the source chip, then
* the src chip coincides with the trg precisely, with no offset.
*/
xOffsetStart = (trgSize/2 - srcSize/2) - (int)(xMEP);
xOffsetEnd = (trgSize/2 - srcSize/2) + (int)(xMEP);
yOffsetStart = (trgSize/2 - srcSize/2) - (int)(yMEP);
yOffsetEnd = (trgSize/2 - srcSize/2) + (int)(yMEP);
/*Allocate working arrays if we haven't already done so.*/
if (s==NULL)
{
s = (complexFloat *)(MALLOC(srcSize*srcSize*sizeof(complexFloat)));
t = (complexFloat *)(MALLOC(trgSize*trgSize*sizeof(complexFloat)));
product = (complexFloat *)(MALLOC(srcSize*srcSize*sizeof(complexFloat)));
peaks=(float *)MALLOC(sizeof(float)*trgSize*trgSize);
}
/*At each grid point, read in a chunk of each image...*/
readMatrix(szImg1,s,FLOAT_COMPLEX,srcSize,srcSize,
x1- srcSize/2+1, y1-srcSize/2+1, samples, lines,0,0);
readMatrix(szImg2,t,FLOAT_COMPLEX,trgSize,trgSize,
x2 - trgSize/2+1, y2-trgSize/2+1, samples, lines,0,0);
/*Take the complex conjugate of the source chunk */
for(y=0;y<srcSize;y++)
{
int srcIndex=y*srcSize;
for(x=0;x<srcSize;x++)
s[srcIndex++].imag*=-1;
}
/*Now compute the best possible offset between these two images,
by checking the phase coherence at each possible offset.*/
peakMax = peakSum = 0.0;
peakMaxX=peakMaxY=count=0;
for(yOffset=yOffsetStart;yOffset<=yOffsetEnd;yOffset++)
{
for(xOffset=xOffsetStart;xOffset<=xOffsetEnd;xOffset++)
{
/* Form an interferogram (multiply by complex conjugate 1
at this offset between the images: */
for(y=0;y<srcSize;y++)
{
int srcIndex=y*srcSize;
int trgIndex=xOffset+(yOffset+y)*trgSize;
for(x=0;x<srcSize;x++)
{
product[srcIndex] = Cmul(s[srcIndex], t[trgIndex]);
srcIndex++,trgIndex++;
}
}
/*Find the phase coherence for this interferogram*/
thisMax=getFFTCorrelation(product,srcSize,srcSize);
/*Possibly save this coherence value.*/
if (thisMax>peakMax)
{
peakMax=thisMax;
peakMaxX=xOffset;
peakMaxY=yOffset;
}
peaks[yOffset*trgSize+xOffset]=thisMax;
peakSum += thisMax;
count++;
}
}
/* Calculate the SNR, with a much faster (but weaker) SNR calculation */
*snr = peakMax / ((peakSum - peakMax) / (float)(count-1))-1.0;
if ((peakMaxX>xOffsetStart)&&(peakMaxY>yOffsetStart)&&
(peakMaxX<xOffsetEnd)&&(peakMaxY<yOffsetEnd))
topOffPeakCpx(peaks,peakMaxX,peakMaxY,trgSize,trgSize,&dx,&dy);
else
dx=dy=0.0;
*peakX=((float)(peakMaxX) + dx - accel1 );
*peakY=((float)(peakMaxY) + dy - accel1 );
}
/*******
amp_corr:
Given two patches, p1 containing the echos from
ahead of beam center, p2 containing echos from behind beam
center; performs an amplitude cross-correlation in range
and returns the range offset, in pixels, between
these two images.
*/
double amp_corr(patch *p1,patch *p2,file *f)
{
int clip=20;/*Remove this from the far-range end of the images*/
int overlap=30;/*Overlap is the maximum shift we ever expect.*/
FILE *fout;
int x,y;
int fftLen=smallestPow2(p1->n_range);
float scale=1.0/fftLen;
complexFloat *amp1,*amp2;
float *corr;
double ret;
amp1=(complexFloat *)MALLOC(sizeof(complexFloat)*fftLen);
amp2=(complexFloat *)MALLOC(sizeof(complexFloat)*fftLen);
corr=(float *)MALLOC(sizeof(float)*(fftLen+overlap));
if (!quietflag) printf(" Performing amplitude correlation.\n");
for (x=0;x<fftLen+overlap;x++)
corr[x]=0.0;
cfft1d(fftLen,NULL,0);
p1->n_range-=clip;
p2->n_range-=clip;
for (y=f->firstOutputLine;y<f->firstOutputLine+f->n_az_valid;y++)
{
for (x=0;x<overlap;x++)
{
amp1[x]=Czero();
amp2[x].r=Cabs(p2->trans[x*p2->n_az+y])*scale;
amp2[x].i=0.0;
}
for (;x<p1->n_range-overlap;x++)
{
amp1[x].r=Cabs(p1->trans[x*p1->n_az+y])*scale;
amp1[x].i=0.0;
amp2[x].r=Cabs(p2->trans[x*p2->n_az+y])*scale;
amp2[x].i=0.0;
}
for (;x<p1->n_range;x++)
{
amp1[x]=Czero();
amp2[x].r=Cabs(p2->trans[x*p2->n_az+y])*scale;
amp2[x].i=0.0;
}
for (;x<fftLen;x++)
amp1[x]=amp2[x]=Czero();
cfft1d(fftLen,amp1,-1);
cfft1d(fftLen,amp2,-1);
for (x=0;x<fftLen;x++)
amp2[x]=Cmul(amp2[x],Cconj(amp1[x]));
cfft1d(fftLen,amp2,1);
for (x=0;x<fftLen;x++)
corr[x]+=Cabs(amp2[x]);
}
fout=FOPEN(f->out_cpx,"w");
for (x=-overlap;x<overlap;x++)
{
corr[x+fftLen]=corr[(x+fftLen)%fftLen];
if (corr[x+fftLen]<10000000000000000000000.0)
/*Is a good value-- print it out*/
fprintf(fout,"%d %f\n",x,corr[x+fftLen]);
else/*Must be an infinity or NaN*/
{
fprintf(stderr, " WARNING!! Found infinity at offset %d!\n",x);
if (logflag) {
sprintf(logbuf," WARNING!! Found infinity at offset %d!\n",x);
printLog(logbuf);
}
corr[x+fftLen]=0.0;
}
}
FCLOSE(fout);
FREE(amp1);
FREE(amp2);
ret = dop_findPeak(corr,fftLen-overlap,2*overlap)-fftLen;
FREE(corr);
return ret;
}
void estdop(char file[],int nDopLines, float *a, float *b,float *c)
{
#define MULTILOOK 100
complexFloat *signal, *signalNext, *dop;
float *x_vec, *y_vec,*phase;
int x,line,firstLine;
getRec *r;
float t1, t2, t3;
long double sum;
float lastPhase;
r=fillOutGetRec(file);
firstLine=(r->nLines-nDopLines)/2;
signal=(complexFloat *)MALLOC(sizeof(complexFloat)*r->nSamples);
signalNext=(complexFloat *)MALLOC(sizeof(complexFloat)*r->nSamples);
dop=(complexFloat *)MALLOC(sizeof(complexFloat)*r->nSamples);
x_vec=(float *)MALLOC(sizeof(float)*r->nSamples);
y_vec=(float *)MALLOC(sizeof(float)*r->nSamples);
phase=(float *)MALLOC(sizeof(float)*r->nSamples);
for (x = 0; x<r->nSamples; x++)
dop[x] = Czero();
getSignalLine(r,firstLine,signalNext,0,r->nSamples);
for (line = firstLine+1; line < firstLine+nDopLines; line++)
{
complexFloat *ptr = signal; signal = signalNext; signalNext = ptr;
getSignalLine(r,line,signalNext,0,r->nSamples);
for (x = 0; x<r->nSamples; x++)
dop[x] = Cadd(dop[x],Cmul(signalNext[x],Cconj(signal[x])));
}
/*Multilook the phase data along range.*/
for (x=0;x<r->nSamples/MULTILOOK;x++)
{
int i;
double out_r=0.0,out_i=0.0;
for (i=0;i<MULTILOOK;i++)
{
out_r+=dop[x*MULTILOOK+i].real;
out_i+=dop[x*MULTILOOK+i].imag;
}
dop[x].real = out_r;
dop[x].imag = out_i;
}
/*Phase-unwrap the doppler values into the "phase" array.*/
lastPhase=0;
for (x=0;x<r->nSamples/MULTILOOK;x++)
{
float nextPhase=atan2(dop[x].imag, dop[x].real)*(1.0/(2.0*pi));
while ((nextPhase-lastPhase)<-0.5) nextPhase+=1.0;
while ((nextPhase-lastPhase)>0.5) nextPhase-=1.0;
phase[x]=nextPhase;
lastPhase=nextPhase;
}
sum = 0.0;
for (x = 0; x<r->nSamples/MULTILOOK; x++)
{
sum += phase[x];
x_vec[x] = x*MULTILOOK;
y_vec[x] = phase[x];
}
// Don't output this file for now since none of our other software uses it
// This file causes the windows uninstaller not to fully uninstall
// if (1) /*Output doppler vs. range*/
// {
// FILE *f=FOPEN("dop_vs_rng","w");
// for (x=0;x<r->nSamples/MULTILOOK;x++)
// fprintf(f,"%.0f\t%f\n",x_vec[x],y_vec[x]);
// fclose(f);
// }
sum = sum / (r->nSamples/MULTILOOK);
if (!quietflag) printf(" Constant Average : y = %f\n",(float)sum);
if (logflag) {
sprintf(logbuf," Constant Average : y = %f\n",(float)sum);
printLog(logbuf);
}
yaxb(x_vec, y_vec, r->nSamples/MULTILOOK, &t1, &t2);
if (!quietflag) printf(" Linear Regression : y = %f x + %f\n",t1,t2);
if (logflag) {
sprintf(logbuf," Linear Regression : y = %f x + %f\n",t1,t2);
printLog(logbuf);
}
yax2bxc(x_vec, y_vec, r->nSamples/MULTILOOK, &t1, &t2, &t3);
if (!quietflag) printf(" Quadratic Regression: y = %f x^2 + %f x + %f\n",t1,t2,t3);
if (logflag) {
sprintf(logbuf," Quadratic Regression: y = %f x^2 + %f x + %f\n",t1,t2,t3);
printLog(logbuf);
}
*a = t3; *b = t2; *c = t1;
free((void *)signal);
free((void *)signalNext);
free((void *)dop);
free((void *)x_vec);
free((void *)y_vec);
free((void *)phase);
freeGetRec(r);
return;
}
static double charact_func0(double k)
{
double X,tau,roeps,u,eps,eps2;
dcomplex Ak,Bk,Ck,Dk,Lambdak,z1,z2,z3,zeta,psi_moins,psi_plus,expo,ans;
dcomplex dlk;
tau = T;
eps = sigma;
roeps = rho*eps;
X = log(S/K) + (r - divid)*tau;
u = kappa - roeps/2.;
eps2 = eps*eps;
if(heston==1)
{
zeta.r = k*k*eps2*(1.-rho*rho) + u*u + eps2/4.;
zeta.i = 2.*k*roeps*u;
zeta = Csqrt(zeta);
psi_moins = Complex(u,roeps*k);
psi_plus = RCmul(-1.,psi_moins);
psi_moins = Cadd(psi_moins,zeta);
psi_plus = Cadd(psi_plus,zeta);
expo = Cexp( RCmul(-tau,zeta) );
z3 = Cadd( psi_moins , Cmul(psi_plus,expo) );
Bk = RCmul( -(k*k+0.25) , Csub(Complex(1.,0),expo) );
Bk = Cdiv(Bk,z3);
Ak = Cdiv( z3 , RCmul(2.,zeta) );
Ak = Clog(Ak);
if(initlog>0)
{
dlk = Csub(Ak,lk_1);
if(dlk.i < -M_PI)
{
bk = bk + 1;
}
else if(dlk.i > M_PI)
{
bk = bk - 1;
}
initlog++;
lk_1 = Ak;
} else {
initlog++;
lk_1 = Ak;
}
Ak = Cadd(Ak, Complex(0.,2*M_PI*bk));
Ak = RCmul( 2. , Ak );
Ak = Cadd( RCmul(tau,psi_plus) , Ak);
Ak = RCmul( -kappa*teta/eps2 , Ak);
}
else
{
Ak = Complex(0.,0.);
Bk = Complex( -0.5*tau*(k*k+0.25) ,0.);
}
if(merton==1)
{
z1 = Complex( 0.5*m0-0.5*v*v*(k*k-0.25) , -k*(m0+0.5*v*v) );
z1 = Cexp(z1);
z2 = Complex(0.5,-k);
z2 = RCmul( exp(m0+0.5*v*v) - 1. , z2);
z2 = Cadd( Complex(1.,0.) , z2 );
Lambdak = Csub(z1,z2);
Ck = Complex(0.,0.);
Dk = RCmul(tau,Lambdak);
}
else
{
Ck = Complex(0.,0.);
Dk = Complex(0.,0.);
}
ans = Cadd( Ak , RCmul(V0,Bk) );
ans = Cadd( ans , Ck );
ans = Cadd( ans , RCmul(lambda0,Dk) );
ans = Cadd( ans , RCmul(X,Complex(0.5,-k) ) );
ans = Cexp(ans);
ans = Cdiv(ans,Complex(k*k+0.25,0.));
if(probadelta == 1)
{
ans = Cmul( ans , Complex(0.5,-k) );
ans = RCmul( 1./S , ans );
}
return ans.r;
}
