void retris(complex *data,complex *a,complex *c, complex *b,
complex endl,complex endr, int nx, complex *d)
{
int ix;
complex *e,den;
complex *f;
e=alloc1complex(nx);
f=alloc1complex(nx);
e[0]=cdiv(cneg(a[0]),endl);
f[0]=cdiv(d[0],endl);
for(ix=1;ix<nx-1;++ix){
den=cadd(b[ix],cmul(c[ix],e[ix-1]));
e[ix]=cdiv(cneg(a[ix]),den);
f[ix]=cdiv(csub(d[ix],cmul(f[ix-1],c[ix])),den);
}
data[nx-1]=cdiv(csub(d[nx-1],cmul(f[nx-2],c[nx-2])),cadd(endr,cmul(c[nx-2],e[nx-2])));
for(ix=nx-2;ix>-1;--ix)
data[ix]=cadd(cmul(data[ix+1],e[ix]),f[ix]);
free1complex(e);
free1complex(f);
return;
}
void nzmg_geod( double e, double n, double *ln, double *lt )
{
complex z0, z1, zn, zd, tmp1, tmp2;
double sum,tmp;
int i, it;
z0.real = (n-n0)/a; z0.imag = (e-e0)/a;
z1.real = cfb2[5].real; z1.imag = cfb2[5].imag;
for (i=5; i--; ) cadd(&z1, cmult(&z1, &z1, &z0), cfb2+i );
cmult(&z1,&z1,&z0);
for(it=2; it--; )
{
cscale( &zn, cfb1+5, 5.0);
cscale( &zd, cfb1+5, 6.0);
for (i=4; i; i--)
{
cadd( &zn, cmult(&tmp1, &zn, &z1), cscale(&tmp2, cfb1+i, (double) i));
cadd( &zd, cmult(&tmp1, &zd, &z1), cscale(&tmp2, cfb1+i, (double) (i+1)));
}
cadd( &zn, &z0, cmult( &zn, cmult( &zn, &zn, &z1), &z1));
cadd( &zd, cfb1, cmult( &zd, &zd, &z1 ));
cdiv( &z1, &zn, &zd );
}
*ln = ln0/rad2deg + z1.imag;
tmp = z1.real;
sum = cfl[8];
for (i=8; i--;) sum = sum*tmp + cfl[i];
sum *= tmp/3600.0e-5;
*lt = (lt0+sum)/rad2deg;
}
void fdm_upconvert(COMP tx_fdm[], int Nc, COMP tx_baseband[NC+1][M], COMP phase_tx[], COMP freq[])
{
int i,c;
COMP two = {2.0, 0.0};
COMP pilot;
for(i=0; i<M; i++) {
tx_fdm[i].real = 0.0;
tx_fdm[i].imag = 0.0;
}
/* Nc/2 tones below centre freq */
for (c=0; c<Nc/2; c++)
for (i=0; i<M; i++) {
phase_tx[c] = cmult(phase_tx[c], freq[c]);
tx_fdm[i] = cadd(tx_fdm[i], cmult(tx_baseband[c][i], phase_tx[c]));
}
/* Nc/2 tones above centre freq */
for (c=Nc/2; c<Nc; c++)
for (i=0; i<M; i++) {
phase_tx[c] = cmult(phase_tx[c], freq[c]);
tx_fdm[i] = cadd(tx_fdm[i], cmult(tx_baseband[c][i], phase_tx[c]));
}
/* add centre pilot tone */
c = Nc;
for (i=0; i<M; i++) {
phase_tx[c] = cmult(phase_tx[c], freq[c]);
pilot = cmult(cmult(two, tx_baseband[c][i]), phase_tx[c]);
tx_fdm[i] = cadd(tx_fdm[i], pilot);
}
/*
Scale such that total Carrier power C of real(tx_fdm) = Nc. This
excludes the power of the pilot tone.
We return the complex (single sided) signal to make frequency
shifting for the purpose of testing easier
*/
for (i=0; i<M; i++)
tx_fdm[i] = cmult(two, tx_fdm[i]);
/* normalise digital oscilators as the magnitude can drfift over time */
for (c=0; c<Nc+1; c++) {
phase_tx[c].real /= cabsolute(phase_tx[c]);
phase_tx[c].imag /= cabsolute(phase_tx[c]);
}
}
/* チャネルのマージ処理 */
static int merge(struct Channel* c2)
{
struct Channel* c1=TBL.ch[0].head;
TBL.ch[6].head=NULL;
TBL.ch[6].tail=NULL;
while(1) {
for(;NTYPE_WAIT!=c1->note.type;c1=c1->next) {
if(cadd(6,&(c1->note))) {
return -1;
}
}
for(;NTYPE_WAIT!=c2->note.type;c2=c2->next) {
if(cadd(6,&(c2->note))) {
return -1;
}
}
if(0==c1->note.val || 0==c2->note.val) {
break;
}
if(c1->note.val < c2->note.val) {
c2->note.val-=c1->note.val;
if(cadd(6,&(c1->note))) {
return -1;
}
c1=c1->next;
}
else if(c1->note.val > c2->note.val){
c1->note.val-=c2->note.val;
if(cadd(6,&(c2->note))) {
return -1;
}
c2=c2->next;
}
else {
if(cadd(6,&(c1->note))) {
return -1;
}
c1=c1->next;
c2=c2->next;
}
}
for(;NTYPE_WAIT!=c1->note.type||0!=c1->note.val;c1=c1->next) {
if(cadd(6,&(c1->note))) {
return -1;
}
}
for(;NTYPE_WAIT!=c2->note.type||0!=c2->note.val;c2=c2->next) {
if(cadd(6,&(c2->note))) {
return -1;
}
}
if(cadd(6,&(c2->note))) {
return -1;
}
TBL.ch[0].head=TBL.ch[6].head;
TBL.ch[0].tail=TBL.ch[6].tail;
TBL.ch[6].head=NULL;
TBL.ch[6].tail=NULL;
return 0;
}
void rx_filter(COMP rx_filt[NC+1][P+1], COMP rx_baseband[NC+1][M+M/P], COMP rx_filter_memory[NC+1][NFILTER], int nin)
{
int c, i,j,k,l;
int n=M/P;
/* rx filter each symbol, generate P filtered output samples for
each symbol. Note we keep filter memory at rate M, it's just
the filter output at rate P */
for(i=0, j=0; i<nin; i+=n,j++) {
/* latest input sample */
for(c=0; c<NC+1; c++)
for(k=NFILTER-n,l=i; k<NFILTER; k++,l++)
rx_filter_memory[c][k] = rx_baseband[c][l];
/* convolution (filtering) */
for(c=0; c<NC+1; c++) {
rx_filt[c][j].real = 0.0; rx_filt[c][j].imag = 0.0;
for(k=0; k<NFILTER; k++)
rx_filt[c][j] = cadd(rx_filt[c][j], fcmult(gt_alpha5_root[k], rx_filter_memory[c][k]));
}
/* make room for next input sample */
for(c=0; c<NC+1; c++)
for(k=0,l=n; k<NFILTER-n; k++,l++)
rx_filter_memory[c][k] = rx_filter_memory[c][l];
}
assert(j <= (P+1)); /* check for any over runs */
}
int
fft_exec (int N, complex * in)
{
int flops;
unsigned int n = N;
unsigned int a, b, i, j, k, r, s;
complex w, p;
flops = 0;
for (i = 1; i < N; i = i * 2)
{
n = n >> 1;
for (k = 0; k < i; k++)
{
w = tableW[k];
r = 2 * n * k;
s = n * (1 + 2 * k);
for (j = 0; j < n; j++)
{
flops += 10;
a = j + r;
b = j + s;
cmult (p, w, in[b]); //6 flop
csub (in[b], in[a], p); //2 flop
cadd (in[a], in[a], p); //2 flop
}
}
}
return flops;
}
void fft_exec (int N, complex * in)
{
unsigned int n = N;
unsigned int a, b, i, j, k, r, s;
complex w, p;
for (i = 1; i < N; i = i * 2)
{
n = n >> 1;
for (k = 0; k < i; k++)
{
w = tableW[k];
r = 2 * n * k;
s = n * (1 + 2 * k);
for (j = 0; j < n; j++)
{
a = j + r/0; // An error
b = j + s;
cmult (p, w, in[b]); //6 flop
csub (in[b], in[a], p); //2 flop
cadd (in[a], in[a], p); //2 flop
}
}
}
}
void cinv22(COMPLEX *a,COMPLEX *b){
double tmp = 1/(a->re*(a+3)->im - (a+1)->im*(a+2)->re);
COMPLEX res1,res2,res3,res4,one;
cmult((a),(a+3),&res1);
cmult((a+1),(a+2),&res2);
res2.re *= -1;
res2.im *= -1;
cadd(&res1,&res2,&res3);
one.re = 1;
one.im = 0;
cdev(&one,&res3,&res4);
cmult(&res4,a+3,b);
cmult(&res4,a+1,b+1);
(b+1)->re *= -1;
(b+1)->im *= -1;
cmult(&res4,a+2,b+2);
(b+2)->re *= -1;
(b+2)->im *= -1;
cmult(&res4,a,b+3);
}
void fft(Complex f[], int len, int t) {
int i, j, k;
Complex e, u, v;
reverse(f, len);
for (i = 2; i <= len; i <<= 1) {
Complex wn;
wn.real = cos(-t * 2 * pi / i);
wn.image = sin(-t * 2 * pi / i);
for (j = 0; j < len; j += i) {
e.real = 1, e.image = 0;
for (k = j; k < j + i / 2; k++) {
u = f[k];
v = cmul(e, f[k+i/2]);
f[k] = cadd(u, v);
f[k+i/2] = csub(u, v);
e = cmul(e, wn);
}
}
}
if (-1 == t) {
for (i = 0; i < len; i++)
f[i].real /= len;
}
}
/**
Convert Closed loop residual PSD back to OL psd using rejection transfer function:
PSD_OL=(PSD_CL-sigma2n/F_nyquist)/Hrej;
*/
dmat *servo_rej2ol(const dmat *psdcl, double dt, long dtrat, double gain, double sigma2n){
SERVO_CALC_T st={0};
servo_calc_init(&st, psdcl, dt, dtrat);
const dmat *nu=st.nu;
const dmat *psd=st.psd;
dmat *psdol=dnew(psd->nx, psd->ny+1);
double psdn=sigma2n*(dt*dtrat*2);
cadd(&st.Hol, 0, st.Hsys, gain);
for(int i=0; i<nu->nx; i++){
dcomplex Hol=st.Hol->p[i];
dcomplex Hrej=1./(1.+Hol);
double normHrej=Hrej*conj(Hrej);
//dcomplex Hcl=Hol*Hrej;
//dcomplex Hwfs=st.Hwfs->p[i];
//dcomplex Hn=Hcl/Hwfs;
//double normHn=Hn*conj(Hn);
IND(psdol, i, 0)=nu->p[i];//frequency.
for(int icol=0; icol<psd->ny; icol++){
IND(psdol, i, icol+1)=IND(psd, i, icol)/(normHrej)-psdn;
if(IND(psdol, i, icol+1)<0){
IND(psdol, i, icol+1)=0;
}
}
}
servo_calc_free(&st);
return psdol;
}
void csqrt( cmplx *z, cmplx *w )
{
cmplx q, s;
double x, y, r, t;
x = z->r;
y = z->i;
if( y == 0.0 )
{
if( x < 0.0 )
{
w->r = 0.0;
w->i = sqrt(-x);
return;
}
else
{
w->r = sqrt(x);
w->i = 0.0;
return;
}
}
if( x == 0.0 )
{
r = fabs(y);
r = sqrt(0.5*r);
if( y > 0 )
w->r = r;
else
w->r = -r;
w->i = r;
return;
}
/* Approximate sqrt(x^2+y^2) - x = y^2/2x - y^4/24x^3 + ... .
* The relative error in the first term is approximately y^2/12x^2 .
*/
if( (fabs(y) < 2.e-4 * fabs(x))
&& (x > 0) )
{
t = 0.25*y*(y/x);
}
else
{
r = cabs(z);
t = 0.5*(r - x);
}
r = sqrt(t);
q.i = r;
q.r = y/(2.0*r);
/* Heron iteration in complex arithmetic */
cdiv( &q, z, &s );
cadd( &q, &s, w );
w->r *= 0.5;
w->i *= 0.5;
}
void lpf_peak_pick(float *foff, float *max, COMP pilot_baseband[],
COMP pilot_lpf[], kiss_fft_cfg fft_pilot_cfg, COMP S[], int nin)
{
int i,j,k;
int mpilot;
COMP s[MPILOTFFT];
float mag, imax;
int ix;
float r;
/* LPF cutoff 200Hz, so we can handle max +/- 200 Hz freq offset */
for(i=0; i<NPILOTLPF-nin; i++)
pilot_lpf[i] = pilot_lpf[nin+i];
for(i=NPILOTLPF-nin, j=0; i<NPILOTLPF; i++,j++) {
pilot_lpf[i].real = 0.0; pilot_lpf[i].imag = 0.0;
for(k=0; k<NPILOTCOEFF; k++)
pilot_lpf[i] = cadd(pilot_lpf[i], fcmult(pilot_coeff[k], pilot_baseband[j+k]));
}
/* decimate to improve DFT resolution, window and DFT */
mpilot = FS/(2*200); /* calc decimation rate given new sample rate is twice LPF freq */
for(i=0; i<MPILOTFFT; i++) {
s[i].real = 0.0; s[i].imag = 0.0;
}
for(i=0,j=0; i<NPILOTLPF; i+=mpilot,j++) {
s[j] = fcmult(hanning[i], pilot_lpf[i]);
}
kiss_fft(fft_pilot_cfg, (kiss_fft_cpx *)s, (kiss_fft_cpx *)S);
/* peak pick and convert to Hz */
imax = 0.0;
ix = 0;
for(i=0; i<MPILOTFFT; i++) {
mag = S[i].real*S[i].real + S[i].imag*S[i].imag;
if (mag > imax) {
imax = mag;
ix = i;
}
}
r = 2.0*200.0/MPILOTFFT; /* maps FFT bin to frequency in Hz */
if (ix >= MPILOTFFT/2)
*foff = (ix - MPILOTFFT)*r;
else
*foff = (ix)*r;
*max = imax;
}
/* thread function */
void* threadfunc(void* arg) {
long long iterations = (long long)arg;
long long i;
if (sync == 'm') {
for (i = 0; i < iterations; i++) {
madd(&counter, 1);
}
for (i = 0; i < iterations; i++) {
madd(&counter, -1);
}
}
else if (sync == 's') {
for (i = 0; i < iterations; i++) {
sadd(&counter, 1);
}
for (i = 0; i < iterations; i++) {
sadd(&counter, -1);
}
}
else if (sync == 'c') {
for (i = 0; i < iterations; i++) {
cadd(&counter, 1);
}
for (i = 0; i < iterations; i++) {
cadd(&counter, -1);
}
}
else {
for (i = 0; i < iterations; i++) {
add(&counter, 1);
}
for (i = 0; i < iterations; i++) {
add(&counter, -1);
}
}
}
bool check_hash(const Crypto::Hash &hash, difficulty_type difficulty) {
uint64_t low, high, top, cur;
// First check the highest word, this will most likely fail for a random hash.
mul(swap64le(((const uint64_t *) &hash)[3]), difficulty, top, high);
if (high != 0) {
return false;
}
mul(swap64le(((const uint64_t *) &hash)[0]), difficulty, low, cur);
mul(swap64le(((const uint64_t *) &hash)[1]), difficulty, low, high);
bool carry = cadd(cur, low);
cur = high;
mul(swap64le(((const uint64_t *) &hash)[2]), difficulty, low, high);
carry = cadc(cur, low, carry);
carry = cadc(high, top, carry);
return !carry;
}
BOOL CProfileAssociationPage::OnCommand(WORD wNotifyCode, WORD wid, HWND hwndControl) {
switch (wid) {
case AddButton :
if (wNotifyCode == BN_CLICKED) {
CAddDeviceDialog cadd(this, m_hwnd);
if (!cadd.bCanceled()) {
UpdateDeviceListBox();
EnableApplyButton();
SettingChanged(TRUE);
}
return TRUE;
}
break;
case RemoveButton :
if (wNotifyCode == BN_CLICKED) {
LRESULT i = SendDlgItemMessage(m_hwnd, DeviceListControl,
LB_GETCURSEL, 0, 0);
if (i == -1)
return TRUE;
unsigned uItem = (unsigned)SendDlgItemMessage(m_hwnd, DeviceListControl,
LB_GETITEMDATA, i, 0);
Dissociate(uItem);
UpdateDeviceListBox();
EnableApplyButton();
SettingChanged(TRUE);
return TRUE;
}
break;
case DeviceListControl :
if (wNotifyCode == LBN_SELCHANGE) {
EnableWindow(GetDlgItem(m_hwnd, RemoveButton),
-1 != SendDlgItemMessage(m_hwnd, DeviceListControl, LB_GETCURSEL, 0, 0));
return TRUE;
}
break;
}
return TRUE;
}
void geod_nzmg( double ln, double lt, double *e, double *n )
{
double sum;
int i;
complex z0,z1;
lt = (lt*rad2deg - lt0) * 3600.0e-5;
sum = cfi[9];;
for (i = 9; i--;) sum = sum*lt+cfi[i];
sum *= lt;
z1.real = sum; z1.imag = ln-ln0/rad2deg;
z0.real = cfb1[5].real; z0.imag = cfb1[5].imag;
for ( i=5; i--;) cadd(&z0,cmult(&z0,&z0,&z1),cfb1+i);
cmult(&z0,&z0,&z1);
*n = n0+z0.real*a;
*e = e0+z0.imag*a;
}
int main()
{
node*temp;
int m,n,i;
node*header1=get_node();
header1->link=NULL;
header1->coeff=header1->expo=-1;
node*header2=get_node();
header2->link=NULL;
header2->coeff=header2->expo=-2;
node*header3=get_node();
header3->link=NULL;
header3->coeff=header3->expo=-3;
printf("Enter the number of terms of the first polynomial\n");
scanf("%d",&m);
for(i=0;i<m;i++)
{
temp=get_node();
printf("Enter the coeff and expo to be inserted\n");
scanf("%d%d",&temp->coeff,&temp->expo);
temp->link=NULL;
insert(header1,temp);
}
printf("Enter the number of terms of the second polynomial\n");
scanf("%d",&n);
for(i=0;i<n;i++)
{
temp=get_node();
printf("Enter the coeff and expo to be inserted\n");
scanf("%d%d",&temp->coeff,&temp->expo);
temp->link=NULL;
insert(header2,temp);
}
printf("\nThe first polynomial is\n");
display(header1);
printf("\nThe second polynomial is\n");
display(header2);
printf("\nThe resultant addition polynomial is\n");
cadd(header1,header2,header3,m,n);
display(header3);
return 0;
}
void CMatMult(COMPLEX *p1,COMPLEX *p2,COMPLEX *p3,int m,int n,int p){//all the matrix in the multiplex must be allot the memory space
int i,j,k;
COMPLEX sum;
//printf("in cmatmult\n");
for( i = 0 ; i < m ; i++ ){
for( j = 0 ; j < p ; j++ ){
sum.re = 0;
sum.im = 0;
for( k = 0 ; k < n ; k++ ){
//printf("ijk:%d %d %d",i,j,k);
COMPLEX tmp;
COMPLEX ori = sum;
cmult((p1+i*n+k),(p2+k*p+j),&tmp);
cadd(&ori,&tmp,&sum);
}
*(p3+i*p+j)= sum; //sprintf(str,"\n%d %d %d %d sum : %lf \n",m,n,i,j,sum.re);qDebug() << str;
}
}
}
t_complex print_cgrid(FILE *fp,char *title,int nx,int ny,int nz,
t_complex ***grid)
{
int ix,iy,iz;
t_complex g,gtot;
gtot=cnul;
if (fp)
fprintf(fp,"Printing all non-zero complex elements of %s\n",title);
for(ix=0; (ix<nx); ix++)
for(iy=0; (iy<ny); iy++)
for(iz=0; (iz<nz); iz++) {
g=grid[ix][iy][iz];
if (fp && ((g.re != 0) || (g.im != 0)))
fprintf(fp,"%s[%2d][%2d][%2d] = %12.5e + i %12.5e\n",
title,ix,iy,iz,g.re,g.im);
gtot = cadd(gtot,g);
}
return gtot;
}
void snr_update(float sig_est[], float noise_est[], COMP phase_difference[])
{
float s[NC+1];
COMP refl_symbols[NC+1];
float n[NC+1];
COMP pi_on_4;
int c;
pi_on_4.real = cosf(PI/4.0);
pi_on_4.imag = sinf(PI/4.0);
/* mag of each symbol is distance from origin, this gives us a
vector of mags, one for each carrier. */
for(c=0; c<NC+1; c++)
s[c] = cabsolute(phase_difference[c]);
/* signal mag estimate for each carrier is a smoothed version of
instantaneous magntitude, this gives us a vector of smoothed
mag estimates, one for each carrier. */
for(c=0; c<NC+1; c++)
sig_est[c] = SNR_COEFF*sig_est[c] + (1.0 - SNR_COEFF)*s[c];
/* noise mag estimate is distance of current symbol from average
location of that symbol. We reflect all symbols into the first
quadrant for convenience. */
for(c=0; c<NC+1; c++) {
refl_symbols[c].real = fabsf(phase_difference[c].real);
refl_symbols[c].imag = fabsf(phase_difference[c].imag);
n[c] = cabsolute(cadd(fcmult(sig_est[c], pi_on_4), cneg(refl_symbols[c])));
}
/* noise mag estimate for each carrier is a smoothed version of
instantaneous noise mag, this gives us a vector of smoothed
noise power estimates, one for each carrier. */
for(c=0; c<NC+1; c++)
noise_est[c] = SNR_COEFF*noise_est[c] + (1 - SNR_COEFF)*n[c];
}
static void checksum(int i, dcomplex u1[NZ][NY][NX], int d[3]) {
/*--------------------------------------------------------------------
c-------------------------------------------------------------------*/
int j, q,r,s, ierr;
dcomplex chk,allchk;
chk.real = 0.0;
chk.imag = 0.0;
#pragma omp for nowait
for (j = 1; j <= 1024; j++) {
q = j%NX+1;
if (q >= xstart[0] && q <= xend[0]) {
r = (3*j)%NY+1;
if (r >= ystart[0] && r <= yend[0]) {
s = (5*j)%NZ+1;
if (s >= zstart[0] && s <= zend[0]) {
cadd(chk,chk,u1[s-zstart[0]][r-ystart[0]][q-xstart[0]]);
}
}
}
}
#pragma omp critical
{
sums[i].real += chk.real;
sums[i].imag += chk.imag;
}
#pragma omp barrier
#pragma omp single
{
/* complex % real */
sums[i].real = sums[i].real/(double)(NTOTAL);
sums[i].imag = sums[i].imag/(double)(NTOTAL);
/* printf("T = %5d Checksum = %22.12e %22.12e\n", */
/* i, sums[i].real, sums[i].imag); */
}
}
extern "C" void positionKernel(const Range& globalID,const Range& localID,CPUImage<Complex>& image,Type& offsetReal,Type& offsetImag,Complex& scaleFactor){
int w = image.getWidth();
int h = image.getHeight();
Complex offset ={
offsetReal, offsetImag
};
float fx = globalID.x, fy = globalID.y;
if(MULTISAMPLING_ENABLED){
uint64_t seed = globalID.x + globalID.y + localID.x + localID.y;
if(MULTISAMPLING_PATTERN == JITTERING){
float xR = uint32_t(seed = random(seed)) / (float)UINT_MAX;
float yR = uint32_t(seed = random(seed)) / (float)UINT_MAX;
fx += xR - 0.5;
fy += yR - 0.5;
}
}
Complex pos = {
floatToType(2 * (fx - w/2) / w),
floatToType(2 * (fy - h/2) / h * (float)h/w)
};
image.at(globalID.x,globalID.y) = cadd(cmul(scaleFactor,pos),offset);
}
int
trans(int c, char *)
{
int f;
if (isalpharune(c) && ft == ITAL && c != 'f' && c != 'j') { /* italic letter */
shim(pclass, nclass = ILET);
cadd(c);
return ITAL;
}
if (isalpharune(c) && ft != ITAL) { /* other letter */
shim(pclass, nclass = OLET);
cadd(c);
return ROM;
}
if (isdigitrune(c)) {
shim(pclass, nclass = DIG);
roman(c);
return ROM; /* this is the right side font of this object */
}
f = ROM;
nclass = OTHER;
switch (c) {
case ':':
case ';':
case '!':
case '%':
case '?':
shim(pclass, nclass);
roman(c);
return f;
case '(':
case '[':
shim(pclass, nclass = LPAR);
roman(c);
return f;
case ')':
case ']':
shim(pclass, nclass = RPAR);
roman(c);
return f;
case ',':
shim(pclass, nclass = OTHER);
roman(c);
return f;
case '.':
if (rf == ROM)
roman(c);
else
cadd(c);
return f;
case '|': /* postscript needs help with default width! */
shim(pclass, nclass = VBAR);
sadd("\\v'.17m'\\z|\\v'-.17m'\\|"); /* and height */
return f;
case '=':
shim(pclass, nclass = PLUS);
sadd("\\(eq");
return f;
case '+':
shim(pclass, nclass = PLUS);
sadd("\\(pl");
return f;
case '>':
case '<': /* >, >=, >>, <, <-, <=, << */
shim(pclass, nclass = PLUS);
if (*psp == '=') {
sadd(c == '<' ? "\\(<=" : "\\(>=");
psp++;
} else if (c == '<' && *psp == '-') { /* <- only */
sadd("\\(<-");
psp++;
} else if (*psp == c) { /* << or >> */
cadd(c);
cadd(c);
psp++;
} else {
cadd(c);
}
return f;
case '-':
shim(pclass, nclass = PLUS); /* probably too big for ->'s */
if (*psp == '>') {
sadd("\\(->");
psp++;
} else {
sadd("\\(mi");
}
return f;
case '/':
shim(pclass, nclass = SLASH);
cadd('/');
return f;
case '~':
case ' ':
sadd("\\|\\|");
return f;
case '^':
sadd("\\|");
return f;
case '\\': /* troff - pass only \(xx without comment */
shim(pclass, nclass);
cadd('\\');
cadd(c = *psp++);
if (c == '(' && *psp && *(psp+1)) {
cadd(*psp++);
cadd(*psp++);
} else
fprintf(stderr, "eqn warning: unquoted troff command \\%c, file %s:%d\n",
c, curfile->fname, curfile->lineno);
return f;
case '\'':
shim(pclass, nclass);
sadd("\\(fm");
return f;
case 'f':
if (ft == ITAL) {
shim(pclass, nclass = ILETF);
cadd('f');
f = ITAL;
} else
cadd('f');
return f;
case 'j':
if (ft == ITAL) {
shim(pclass, nclass = ILETJ);
cadd('j');
f = ITAL;
} else
cadd('j');
return f;
default:
shim(pclass, nclass);
cadd(c);
return ft==ITAL ? ITAL : ROM;
}
}
/************************PSPI migration************************/
void pspimig(float **data,complex **image,float **v,int nt,int nx,int nz,float dt,float dx, float dz)
{
int ntfft; /*number of samples of the zero padded trace*/
int nxfft;
int nw; /*number of temporal freqs.*/
int it; /*loop index over time sample*/
int ix; /*loop index over midpoint sample*/
int iw; /*loop index over frequency*/
int ik; /*loop index over wavenumber*/
int iz; /*loop index over migrated depth samples*/
int iv; /*loop index over reference velocities*/
int nvref_max=8; /*number of reference velocities in each layer*/
int nvref;
int i1; /*nearest reference velocity*/
int i2;
int iw1;
int iw2;
float **vref; /*2D reference velocity array*/
float w0; /*first frequency sample*/
float w; /*frequency*/
float dw; /*frequency sampling interval*/
float k0; /*first wavenumber*/
float k;
float dk; /*wave number sampling interval in x*/
float dv; /*velocity interval*/
float *in; /*input 1D data in FFTW*/
float **cpdata;
float phase;
float wv;
float vmin;
float vmax;
float f1 = 1.0;
float f2 = 35.0;
complex *out; /*output of 1D FFT using FFTW*/
complex **datawx; /*data in frequency-wavenumber domain*/
complex *in2;
complex *out2;
complex *in3;
complex *out3;
complex **Pkv; /*wavefield in k-v*/
complex **Pxv; /*wavefield in x-v*/
complex **Pwx; /*wavefield in w-x*/
complex cshift;
complex tmp_a;
complex tmp_b;
fftwf_plan p1;
fftwf_plan p2;
fftwf_plan p3;
/*allocate memory of reference velocities*/
vref = alloc2float(nz,nvref_max); /*allocate 2D array for reference velocity to avoid changing memory of vector*/
/*nz by nvref_max*/
for (ix=0;ix<nx;ix++){
for (iz=0;iz<nz;iz++){
image[ix][iz] = cmplx(0.0,0.0); /*nz by nz*/
}
}
/*devide velocity by 2 for downward continuation*/
for (iz=0;iz<nz;iz++){
for (ix=0;ix<nx;ix++){
v[ix][iz]=v[ix][iz]/2.0;
}
}
/*fprintf(stderr,"nx=%d nz=%d",nx,nz);
FILE *Fvp = fopen("vel.bin", "wb");
fwrite(v[0],1,4*nx*nz,Fvp);
fclose(Fvp);*/
/*zero padding in termporal direction*/
ntfft = 1.0*exp2(ceil(log2(nt))); /*number of zero padded trace in FFT*/
nw = ntfft/2+1; /*number of points of frequency axis after FFTW*/
cpdata = alloc2float(ntfft,nx); /*data after zero padding*/
for (ix=0;ix<nx;ix++){
for (it=0;it<ntfft;it++){
if(it<=nt) cpdata[ix][it] = data[ix][it];
else {cpdata[ix][it] = 0.0;
}
}
}
/*allocate memory for w-x domain data*/
datawx = alloc2complex(nw,nx); /*w-x data nx by nw*/
iw1 = floor(f1*dt*ntfft)+1;
iw2 = floor(f2*dt*ntfft)+1;
/*define plans for FFT using FFTW*/
/*plan 1 from t-x to w-x*/
in = alloc1float(ntfft);
out = alloc1complex(nw);
p1 = fftwf_plan_dft_r2c_1d(ntfft,in,(fftwf_complex*)out,FFTW_ESTIMATE); /*real to complex*/
/*plan 2 from w-x to w-k*/
nxfft = 1.0*exp2(ceil(log2(nx)));
in2 = alloc1complex(nxfft);
out2 = alloc1complex(nxfft);
p2 = fftwf_plan_dft_1d(nxfft,(fftwf_complex*)in2,(fftwf_complex*)out2,FFTW_FORWARD,FFTW_ESTIMATE);
/*plan 3 from w-k to w-x*/
in3 = alloc1complex(nxfft);
out3 = alloc1complex(nxfft);
p3 = fftwf_plan_dft_1d(nxfft,(fftwf_complex*)in3,(fftwf_complex*)out3,FFTW_BACKWARD,FFTW_ESTIMATE);
Pxv = alloc2complex(nvref_max,nxfft);
Pkv = alloc2complex(nvref_max,nxfft);
Pwx = alloc2complex(nw,nxfft);
/*apply first 1-D Fourier transform on data from t-x to w-x using FFTW package*/
for (ix=0;ix<nx;ix++){
for(it=0;it<ntfft;it++){
in[it] = cpdata[ix][it]; /*assign one trace to a vector*/
}
fftwf_execute(p1);
for(iw=0;iw<nw;iw++){
datawx[ix][iw] = cdiv(out[iw], cmplx(sqrt(ntfft), 0.0));
} /*w*/
} /*x*/
fftwf_destroy_plan(p1);
/*determine frequency and wavenumber axis*/
dw = 2.0*PI/(ntfft*dt); /*frequency sampling interval*/
w0 = 0.0; /*first frequency sample*/
dk = 2.0*PI/(nxfft*dx); /*wavenumber sampling interval*/
k0 = 0.0; /*first wavenumber sample*/
/*initialization of downward wavefield*/
for (iw=0;iw<nw;iw++){
for (ix=0;ix<nxfft;ix++){
if (ix<nx){Pwx[ix][iw] = datawx[ix][iw];}
else{Pwx[ix][iw] = cmplx(0.0,0.0);}
}
}
/*loop over depth z*/
for (iz=0;iz<nz;iz++){
fprintf(stderr,"depth sample %d\n",iz);
/*calculate reference velocities of each layer*/
vmin = v[0][iz];
vmax = v[0][iz];
for (ix=0;ix<nx;ix++){
if(v[ix][iz]>=vmax) vmax=v[ix][iz]; /*get the maximum velocity*/
if(v[ix][iz]<=vmin) vmin=v[ix][iz]; /*get the minimum velocity*/
}
dv = (vmax-vmin)/(nvref_max-1);
if(dv/vmax<=0.001){
nvref = 1;
vref[0][iz]=(vmin+vmax)/2;
}
else
{
nvref = nvref_max;
for (iv=0;iv<nvref_max;iv++)
{
vref[iv][iz] = vmin+dv*iv;
}
}
/*loop over frequencies*/
w = w0;
for (iw=iw1;iw<=iw2;iw++){
w = w0 + iw*dw; /*frequency axis (important)*/
/*apply phase-shift in w-x (optional)*/
/*datawx*/
/*Apply second FFT to tranform w-x data to w-k domain using FFTW*/
for (ix=0;ix<nxfft;ix++){
in2[ix] = Pwx[ix][iw];
}
fftwf_execute(p2);
for (ik=0;ik<nxfft;ik++){
out2[ik] = cdiv(out2[ik], cmplx(sqrt(nxfft), 0.0));
}
/*loop over wavenumbers*/
k = k0;
for (ik=0;ik<nxfft;ik++){
if (ik<=nxfft/2){
k = ik*dk; /*wavenumber axis (important)*/
}
else{
k = (ik-nxfft)*dk;
}
/*loop over reference velocities*/
for (iv=0;iv<nvref;iv++){
wv = w/vref[iv][iz];
if(wv>fabs(k)){ /*note that k can be negative*/
phase = sqrt(wv*wv-k*k)*dz;
cshift = cmplx(cos(phase),sin(phase));
}
else{
cshift = cmplx(0.0,0.0);
}
Pkv[ik][iv] = cmul(out2[ik],cshift);
} /*end for v*/
} /*end for k*/
/*from w-k go back to w-x domain*/
for (iv=0;iv<nvref;iv++){ /*inverse FFT for each velocity*/
for (ik=0;ik<nxfft;ik++){
in3[ik] = Pkv[ik][iv];
} /*end for k*/
fftwf_execute(p3);
for (ix=0;ix<nxfft;ix++){
Pxv[ix][iv] = cdiv(out3[ix], cmplx(sqrt(nxfft), 0.0));
} /*end for x*/
} /*end for v*/ /*Pxv ix by iv*/
/*interpolation of wavefield in w-x*/
if (nvref==1){
for (ix=0;ix<nx;ix++){
Pwx[ix][iw] = Pxv[ix][0];
}
}
else
{
for (ix=0;ix<nx;ix++){
if (v[ix][iz]==vmax){i1=(v[ix][iz]-vmin)/dv-1;}
else
{i1 = (v[ix][iz]-vmin)/dv;} /*find nearest reference velocity and wavefield*/
i2 = i1+1;
tmp_a = cadd(crmul(Pxv[ix][i1], vref[i2][iz]-v[ix][iz]) , crmul(Pxv[ix][i2], v[ix][iz]-vref[i1][iz]));
tmp_b = cmplx(vref[i2][iz]-vref[i1][iz], 0.0);
Pwx[ix][iw] = cdiv(tmp_a,tmp_b);
} /*interpolate wavefield*/
} /*end else*/
/*imaging condition*/
for (ix=0;ix<nx;ix++){
image[ix][iz] = cadd(image[ix][iz],Pwx[ix][iw]);
}
/*zero padding*/
for (ix=nx;ix<nxfft;ix++){
Pwx[ix][iw] = cmplx(0.0,0.0);
}
} /*w*/
} /*z*/
fftwf_destroy_plan(p2);
fftwf_destroy_plan(p3);
} /*end pspimig migration function*/
int ChebyshevFilter::zplna()
{
cmplx r, cnum, cden, cwc, ca, cb, b4ac;
double C;
if( kind == 3 )
C = c;
else
C = wc;
for( i=0; i<ARRSIZ; i++ )
{
z[i].r = 0.0;
z[i].i = 0.0;
}
nc = np;
jt = -1;
ii = -1;
for( icnt=0; icnt<2; icnt++ )
{
/* The maps from s plane to z plane */
do
{
ir = ii + 1;
ii = ir + 1;
r.r = zs[ir];
r.i = zs[ii];
switch( type )
{
case 1:
case 3:
/* Substitute s - r = s/wc - r = (1/wc)(z-1)/(z+1) - r
*
* 1 1 - r wc ( 1 + r wc )
* = --- -------- ( z - -------- )
* z+1 wc ( 1 - r wc )
*
* giving the root in the z plane.
*/
cnum.r = 1 + C * r.r;
cnum.i = C * r.i;
cden.r = 1 - C * r.r;
cden.i = -C * r.i;
jt += 1;
cdiv( &cden, &cnum, &z[jt] );
if( r.i != 0.0 )
{
/* fill in complex conjugate root */
jt += 1;
z[jt].r = z[jt-1 ].r;
z[jt].i = -z[jt-1 ].i;
}
break;
case 2:
case 4:
/* Substitute s - r => s/wc - r
*
* z^2 - 2 z cgam + 1
* => ------------------ - r
* (z^2 + 1) wc
*
* 1
* = ------------ [ (1 - r wc) z^2 - 2 cgam z + 1 + r wc ]
* (z^2 + 1) wc
*
* and solve for the roots in the z plane.
*/
if( kind == 2 )
cwc.r = cbp;
else
cwc.r = c;
cwc.i = 0.0;
cmul( &r, &cwc, &cnum ); /* r wc */
csub( &cnum, &cone, &ca ); /* a = 1 - r wc */
cmul( &cnum, &cnum, &b4ac ); /* 1 - (r wc)^2 */
csub( &b4ac, &cone, &b4ac );
b4ac.r *= 4.0; /* 4ac */
b4ac.i *= 4.0;
cb.r = -2.0 * cgam; /* b */
cb.i = 0.0;
cmul( &cb, &cb, &cnum ); /* b^2 */
csub( &b4ac, &cnum, &b4ac ); /* b^2 - 4 ac */
csqrt( &b4ac, &b4ac );
cb.r = -cb.r; /* -b */
cb.i = -cb.i;
ca.r *= 2.0; /* 2a */
ca.i *= 2.0;
cadd( &b4ac, &cb, &cnum ); /* -b + sqrt( b^2 - 4ac) */
cdiv( &ca, &cnum, &cnum ); /* ... /2a */
jt += 1;
cmov( &cnum, &z[jt] );
if( cnum.i != 0.0 )
{
jt += 1;
z[jt].r = cnum.r;
z[jt].i = -cnum.i;
}
if( (r.i != 0.0) || (cnum.i == 0) )
{
csub( &b4ac, &cb, &cnum ); /* -b - sqrt( b^2 - 4ac) */
cdiv( &ca, &cnum, &cnum ); /* ... /2a */
jt += 1;
cmov( &cnum, &z[jt] );
if( cnum.i != 0.0 )
{
jt += 1;
z[jt].r = cnum.r;
z[jt].i = -cnum.i;
}
}
} /* end switch */
}
while( --nc > 0 );
if( icnt == 0 )
{
zord = jt+1;
if( nz <= 0 )
{
if( kind != 3 )
return(0);
else
break;
}
}
nc = nz;
} /* end for() loop */
return 0;
}
void gazdagvt (float k,
int nt, float dt, float ft,
int ntau, float dtau, float ftau,
float *vt, complex *p, complex *q, float qual, float gainceil)
/*****************************************************************************
Gazdag's phase-shift zero-offset migration for one wavenumber
adapted to v(tau) velocity profile
******************************************************************************
Input:
k wavenumber
nt number of time samples
dt time sampling interval
ft first time sample
ntau number of migrated time samples
dtau migrated time sampling interval
ftau first migrated time sample
vt velocity v[tau]
p array[nt] containing data to be migrated
Output:
q array[ntau] containing migrated data
******************************************************************************/
{
int ntfft,nw,it,itau,iw;
float dw,fw,tmax,w,tau,phase,coss, *cumgain, gain, alpha;
complex cshift,*pp;
/* determine frequency sampling */
ntfft = npfa(nt);
nw = ntfft;
dw = 2.0*PI/(ntfft*dt);
fw = -PI/dt;
/* determine maximum time */
tmax = ft+(nt-1)*dt;
/* allocate workspace */
pp = alloc1complex(nw);
cumgain = alloc1float(nw);
for (iw=0; iw<nw; iw++)
cumgain[iw] = 1.0;
/* pad with zeros and Fourier transform t to w, with w centered */
for (it=0; it<nt; it++)
pp[it] = (it%2 ? cneg(p[it]) : p[it]);
for (it=nt; it<ntfft; it++)
pp[it] = cmplx(0.0,0.0);
pfacc(1,ntfft,pp);
/* account for non-zero ft and non-zero ftau */
for (itau=0 ; itau < ftau ; itau++){
for (iw=0,w=fw; iw<nw; iw++,w+=dw) {
if (w==0.0) w = 1e-10/dt;
coss = 1.0-pow(0.5 * vt[itau] * k/w,2.0);
if (coss>=pow(ftau/tmax,2.0)) {
phase = w*(ft-ftau*sqrt(coss));
cshift = cmplx(cos(phase),sin(phase));
pp[iw] = cmul(pp[iw],cshift);
} else {
pp[iw] = cmplx(0.0,0.0);
}
}
}
/* loop over migrated times tau */
for (itau=0,tau=ftau; itau<ntau; itau++,tau+=dtau) {
/* initialize migrated sample */
q[itau] = cmplx(0.0,0.0);
/* loop over frequencies w */
for (iw=0,w=fw; iw<nw; iw++,w+=dw) {
/* accumulate image (summed over frequency) */
q[itau] = cadd(q[itau],pp[iw]);
/* compute cosine squared of propagation angle */
if (w==0.0) w = 1e-10/dt;
coss = 1.0-pow(0.5 * vt[itau] * k/w,2.0);
/* if wave could have been recorded in time */
if (coss>=pow(tau/tmax,2.0)) {
/* extrapolate down one migrated time step */
phase = -w*dtau*sqrt(coss);
cshift = cmplx(cos(phase),sin(phase));
/* apply gain until gain ceiling is reached */
if (cumgain[iw] < gainceil) {
alpha = w/(2.0*vt[itau]*qual);
gain = exp(fabs(0.5*vt[itau]*dtau*alpha));
pp[iw] = cmul(pp[iw],crmul(cshift,gain));
cumgain[iw] *= gain;
} else {
pp[iw] = cmplx(0.0,0.0);
}
/* else, if wave couldn't have been recorded in time */
} else {
/* zero the wave */
pp[iw] = cmplx(0.0,0.0);
}
}
/* scale accumulated image just as we would for an FFT */
q[itau] = crmul(q[itau],1.0/nw);
}
/* free workspace */
free1complex(pp);
free1float(cumgain);
}
void fdmig( complex **cp, int nx, int nw, float *v,float fw,float
dw,float dz,float dx,float dt,int dip)
{
int iw,ix,step=1;
float *s1,*s2,w,coefa[5],coefb[5],v1,vn,trick=0.1;
complex cp2,cp3,cpnm1,cpnm2;
complex a1,a2,b1,b2;
complex endl,endr;
complex *data,*d,*a,*b,*c;
s1=alloc1float(nx);
s2=alloc1float(nx);
data=alloc1complex(nx);
d=alloc1complex(nx);
a=alloc1complex(nx);
b=alloc1complex(nx);
c=alloc1complex(nx);
if(dip==45){
coefa[0]=0.5;coefb[0]=0.25;
step=1;
}
if(dip==65){
coefa[0]=0.478242060;coefb[0]=0.376369527;
step=1;
}
if(dip==79){
coefa[0]=coefb[0]=0.4575;
step=1;
}
if(dip==80){
coefa[1]=0.040315157;coefb[1]=0.873981642;
coefa[0]=0.457289566;coefb[0]=0.222691983;
step=2;
}
if(dip==87){
coefa[2]=0.00421042;coefb[2]=0.972926132;
coefa[1]=0.081312882;coefb[1]=0.744418059;
coefa[0]=0.414236605;coefb[0]=0.150843924;
step=3;
}
if(dip==89){
coefa[3]=0.000523275;coefb[3]=0.994065088;
coefa[2]=0.014853510;coefb[2]=0.919432661;
coefa[1]=0.117592008;coefb[1]=0.614520676;
coefa[0]=0.367013245;coefb[0]=0.105756624;
step=4;
}
if(dip==90){
coefa[4]=0.000153427;coefb[4]=0.997370236;
coefa[3]=0.004172967;coefb[3]=0.964827992;
coefa[2]=0.033860918;coefb[2]=0.824918565;
coefa[1]=0.143798076;coefb[1]=0.483340757;
coefa[0]=0.318013812;coefb[0]=0.073588213;
step=5;
}
v1=v[0];vn=v[nx-1];
do {
step--;
for(iw=0,w=fw;iw<nw;iw++,w+=dw){
if(fabs(w)<=1.0e-10)w=1.0e-10/dt;
for(ix=0;ix<nx;ix++){
s1[ix]=(v[ix]*v[ix])*coefb[step]/(dx*dx*w*w)+trick;
s2[ix]=-v[ix]*dz*coefa[step]/(w*dx*dx)*0.5;
}
for(ix=0;ix<nx;ix++){
data[ix]=cp[iw][ix];
}
cp2=data[1];
cp3=data[2];
cpnm1=data[nx-2];
cpnm2=data[nx-3];
a1=crmul(cmul(cp2,conjg(cp3)),2.0);
b1=cadd(cmul(cp2,conjg(cp2)),cmul(cp3,conjg(cp3)));
if(b1.r==0.0 && b1.i==0.0)
a1=cwp_cexp(cmplx(0.0,-w*dx*0.5/v1));
else
a1=cdiv(a1,b1);
if(a1.i>0.0)a1=cwp_cexp(cmplx(0.0,-w*dx*0.5/v1));
a2=crmul(cmul(cpnm1,conjg(cpnm2)),2.0);
b2=cadd(cmul(cpnm1,conjg(cpnm1)),cmul(cpnm2,conjg(cpnm2)));
if(b2.r==0.0 && b2.i==0.0)
a2=cwp_cexp(cmplx(0.0,-w*dx*0.5/vn));
else
a2=cdiv(a2,b2);
if(a2.i>0.0)a2=cwp_cexp(cmplx(0.0,-w*dx*0.5/vn));
for(ix=0;ix<nx;ix++){
a[ix]=cmplx(s1[ix],s2[ix]);
b[ix]=cmplx(1.0-2.0*s1[ix],-2.0*s2[ix]);
}
for(ix=1;ix<nx-1;ix++){
d[ix]=cadd(cadd(cmul(data[ix+1],a[ix+1]),cmul(data[ix-1],a[ix-1])),
cmul(data[ix],b[ix]));
}
d[0]=cadd(cmul(cadd(b[0],cmul(a[0],a1)),data[0]),cmul(data[1],a[1]));
d[nx-1]=cadd(cmul(cadd(b[nx-1],cmul(a[nx-1],a2)),data[nx-1]),
cmul(data[nx-2],a[nx-2]));
for(ix=0;ix<nx;ix++){
data[ix]=cmplx(s1[ix],-s2[ix]);
b[ix]=cmplx(1.0-2.0*s1[ix],2.0*s2[ix]);
}
endl=cadd(b[0],cmul(data[0],a1));
endr=cadd(b[nx-1],cmul(data[nx-1],a2));
for(ix=1;ix<nx-1;ix++){
a[ix]=data[ix+1];
c[ix]=data[ix-1];
}
a[0]=data[1];
c[nx-1]=data[nx-2];
retris(data,a,c,b,endl,endr,nx,d);
for(ix=0;ix<nx;ix++){
cp[iw][ix]=data[ix];
}
}
}while(step);
free1complex(data);
free1complex(d);
free1complex(b);
free1complex(c);
free1complex(a);
free1float(s1);
free1float(s2);
return;
}
int main(void)
{
/* *************************************************************************
Create Window
********************************************************************** */
wwB = 1000;
wid = pageb(100, 0, 900, wwB, "B-mode");
start();
Bx_ini = 100; By_ini = 100;
/**************************************************************************
Input Serial No.
********************************************************************** */
//printf("input beginning and end of #serial\n");
//scanf("%d %d", &iseq1);
iseq1 = 101;
printf("iseq1 = %d \n", iseq1);
printf("*******************************\n");
printf("* start analysing *\n");
printf("*******************************\n");
/* *************************************************************************
Reading Parameters from File
********************************************************************** */
start:;
if ((fin = fopen("./ssd6500.coe", "r")) == NULL){
fprintf(stderr, "cannot open dpman1M.coe\n");
exit(1);
}
icfile = 0;
while (icfile == 0){
fscanf(fin, "%d %s %d %d %s %d %f %d %d %d %d",
&islct00, name, &iseq, &nfhead, fname, &iage, &rryy, &icnt, &ip_s, &ip_d,
&anabt);
if (iseq == iseq1) icfile = 1;
if (islct00 == 9){ fclose(fin); exit(0); }
}
fclose(fin);
rmaxdh = 1e-6*icnt;
printf("islct00 = %d\n", islct00);
printf("Name of subject = %s\n", name);
printf("Serial number = %d\n", iseq);
printf("Age of subject = %d years old\n", iage);
printf("Maximal velocity range = %f m/s\n", rryy);
printf("Systolic & Diastolic blood pressure = %d, %d mmHg\n", ip_s, ip_d);
/* *************************************************************************
Read Raw Data from File
********************************************************************** */
sprintf(fall, "%s%s.rfa", fhead[nfhead], fname);
printf("File name = %s\n", fall);
if ((fin = fopen(fall, "rb")) == NULL){
printf("cannot open file: %s\n", fall);
exit(1);
}
fseek(fin, 480L, SEEK_SET);
//fread(buf,sizeof(char),480,fin);
fread(&fdat, sizeof(float), 1, fin); /* 送信周波数 [MHz] */
printf("f0 = %f Hz\n", f0 = 1e6*fdat);
fread(&fdat, sizeof(float), 1, fin); /* Burst波数 */
fread(&usdat, sizeof(unsigned short), 1, fin);
npb = usdat;
fread(&fdat, sizeof(float), 1, fin); /* PRF [kHz] */
printf("PRF = %f Hz\n", 1e3*fdat);
fread(&fdat, sizeof(float), 1, fin); /* 受信サンプリング周波数 [MHz] */
printf("fs = %f Hz\n", fs = 1e6*fdat);
fread(&fdat, sizeof(float), 1, fin); /* ビーム間隔 [mm] */
printf("beam interval = %f mm\n", elp = fdat);
fread(&usdat, sizeof(unsigned short), 1, fin); /* ビーム数 */
printf("number of beams = %d\n", nposi = usdat);
fread(&usdat, sizeof(unsigned short), 1, fin); /* ROI上端深さ(point)=0 */
printf("record offset = %d\n", usdat);
fread(&usdat, sizeof(unsigned short), 1, fin); /* ビーム数当たりサンプル数 */
printf("number of samples per line = %d\n", ncount = usdat);
fread(&usdat, sizeof(unsigned short), 1, fin); /* 開始ビーム番号 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* 終了ビーム番号 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* フォーカス段数 */
for (i = 0; i<7; ++i) fread(&fdat, sizeof(float), 1, fin); /* フォーカス位置設定 */
for (i = 0; i<4; ++i)
fread(&usdat, sizeof(unsigned short), 1, fin); /* フォーカス音圧設定 */
fread(&fdat, sizeof(float), 1, fin); /* ステア角 [degree] */
printf("steering angle: %f\n", fdat);
fread(&fdat, sizeof(float), 1, fin); /* フレームレート [Hz] */
printf("frame rate = %f Hz\n", PRF2 = fdat);
fread(&fdat, sizeof(float), 1, fin); /* スケールファクター */
fread(&uldat, sizeof(unsigned long), 1, fin); /* フレーム数 */
printf("number of frames = %d\n", nframe = uldat);
fread(&usdat, sizeof(unsigned short), 1, fin); /* データ情報 */
fread(&fdat, sizeof(float), 1, fin); /* 受信周波数 [MHz] */
//printf("receiving frequency: %f MHz\n",fdat);
for (i = 0; i<14; ++i) fread(&cdat, sizeof(char), 1, fin); /* 未使用 */
for (i = 0; i<4; ++i) fread(&usdat, sizeof(unsigned short), 1, fin); /* 諸設定 */
for (i = 0; i<16; ++i) fread(&cdat, sizeof(char), 1, fin); /* Sweepファイル名 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* bit resolution */
fread(&usdat, sizeof(unsigned short), 1, fin); /* 波形種別 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* 波形関数 */
fread(&fdat, sizeof(float), 1, fin); /* 振幅補正 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* データ数 */
fread(&fdat, sizeof(float), 1, fin); /* 比帯域 */
//fread(&fdat,sizeof(float),1,fin); /* Flat比率 */
//fread(&fdat,sizeof(float),1,fin); /* 送信波形 台 */
for (i = 0; i<8; ++i) fread(&cdat, sizeof(char), 1, fin); /* スペア */
fread(&fdat, sizeof(float), 1, fin); /* 送信サンプル周波数 [MHz] */
fread(&usdat, sizeof(unsigned short), 1, fin); /* Ex-PHDのon/off */
fread(&usdat, sizeof(unsigned short), 1, fin); /* B FE-Gain上限 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* B FE-Gainの下限 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* B-Gain */
fread(&usdat, sizeof(unsigned short), 1, fin); /* 送信回数 */
printf("number of transmissions per line: %d\n", usdat);
fread(&usdat, sizeof(unsigned short), 1, fin); /* アライメント */
fread(&usdat, sizeof(unsigned short), 1, fin); /* プレーン識別番号 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* 空間コンパウンド識別番号 */
printf("spatial compound: %d\n", usdat);
fread(&fdat, sizeof(float), 1, fin); /* 音響パワー */
fread(&usdat, sizeof(unsigned short), 1, fin); /* Interm Frame */
fread(&fdat, sizeof(float), 1, fin); /* Interm Inverval */
fread(&usdat, sizeof(unsigned short), 1, fin); /* エラスト識別番号 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* RFビット位置 */
for (i = 0; i<4; ++i) fread(&cdat, sizeof(char), 1, fin);
//fread(&uldat,sizeof(unsigned long),1,fin);
/*printf("number of data = %d bytes\n",uldat);*/
/*printf("%d bytes\n",4*4+4*ncount*nposi*nframe);*/
fseek(fin, 996L, SEEK_SET); //932
// loading RF data
tcnt = (4 + ncount)*nposi*nframe;
raw_dat = (int *)malloc((size_t)(4 * tcnt));
fread(raw_dat, sizeof(int), tcnt, fin);
printf("raw size = %f\n", sizeof(raw_dat) / sizeof(int));
// loading physio data (ECG)
fread(&idcnt, sizeof(int), 1, fin);
icnt = (idcnt & 0x3FFFC00) >> 6;
printf("icnt = %d\n", icnt);
for (i = 0; i<3; ++i) fread(&idcnt, sizeof(int), 1, fin);
for (i = 0; i<icnt; ++i){
fread(&emin, sizeof(unsigned short), 1, fin);
fread(&emax, sizeof(unsigned short), 1, fin);
fread(&pmin, sizeof(unsigned short), 1, fin);
fread(&pmax, sizeof(unsigned short), 1, fin);
fread(&plmin, sizeof(unsigned short), 1, fin);
fread(&plmax, sizeof(unsigned short), 1, fin);
for (j = 0; j<18; ++j) fread(&idcnt, sizeof(unsigned short), 1, fin);
idcnt = PRF2*i / 1000.;
ecg[i] = emax;
pcg[i] = plmax;
}
fclose(fin);
fin = fopen("./ecg.dat", "w");
for (i = 0; i<icnt; ++i) fprintf(fin, "%f\n", ecg[i]);
fclose(fin);
/*fin=fopen("./pcg.dat","w");
for(i=0;i<icnt;++i) fprintf(fin,"%f\n",pcg[i]);
fclose(fin);*/
for (i = 0; i<nframe; ++i){
for (j = 0; j<nposi; ++j){
ihof[j][i] = (4 + ncount)*j + (4 + ncount)*nposi*i;
}
}
nfr_low = nframe;
printf("interval of beams = %f mm\n", elp);
dadd = 4 * 2 * elp / 0.15;
/* Post ECG */
for (i = 0; i<nframe; ++i){
MpF = (int)(1000 / PRF2*i);
ecgmax = ecg[MpF];
ecg[i] = ecgmax;
}
fin = fopen("./ecg2.dat", "w");
for (i = 0; i<nframe; ++i) fprintf(fin, "%f\n", ecg[i]);
fclose(fin);
/* *************************************************************************
Setting Parameters (Temporal)
********************************************************************** */
if (aux_flg == 0) iaux = 5;
if (aux_flg == 1) iaux = 6;
f0i = f0;
ifs = (int)(fs / 1e6);
nspc = 1;
if (SPAv == 1) nspc = 1;
/* *************************************************************************
Setting Parameters
********************************************************************** */
heartrate = 100;
p_s = (float)ip_s;
p_d = (float)ip_d;
icgain = 4000000;
idltx = 10; /* nwv wavelength (default:4x) */
idltx2 = 10;
deltad = c0 / (fs*2.0);
lp2s = (int)(2.0 / f0*fs);
lp2q = (int)(2.0 / f0*fs); /* cut off frequency in quadrature demodulation */
//f0=5.5*1e+6;
/* *************************************************************************
Memory Allocation
********************************************************************** */
if (ncount >= nframe){
dmy = (float *)malloc((size_t)(8 * (ncount + 1)));
sgn = (float *)malloc((size_t)(8 * (ncount + 1)));
}
if (nframe>ncount){
dmy = (float *)malloc((size_t)(8 * (nframe + 1)));
sgn = (float *)malloc((size_t)(8 * (nframe + 1)));
}
/* *************************************************************************
Read ECG, PCG, AUX
********************************************************************** */
ecgbias = 0.0; icnt = 0;
for (i = 0; i<nframe / 2; ++i){
ecgbias = ecgbias + ecg[i];
++icnt;
}
for (i = 0; i<nframe; ++i) ecg[i] = ecg[i] - ecgbias / (float)icnt;
ecgmax = 0.0;
for (i = 0; i<nframe / 2; ++i){
if (fabs(ecg[i])>ecgmax) ecgmax = fabs(ecg[i]);
}
pcgbias = 0.0; icnt = 0;
for (i = 0; i<nframe / 2; ++i){
pcgbias = pcgbias + pcg[i];
++icnt;
}
for (i = 0; i<nframe; ++i) pcg[i] = pcg[i] - pcgbias / (float)icnt;
pcgmax = 0.0;
for (i = 0; i<nframe / 2; ++i){
if (fabs(pcg[i])>pcgmax) pcgmax = fabs(pcg[i]);
}
/* ****************************************************************************
Detect Timing of R-wave
************************************************************************* */
nbeat = 0; beat = 0.0;
ilevel = 0.6*ecgmax;
period = 1.0 / (heartrate / 60.0);
for (j = 0; j<maxfrm; ++j){
if (j == 0) ii1 = 0;
if (j >= 1) ii1 = track0[j - 1] + (int)(PRF2*period);
if (ii1>nframe) break;
for (i = ii1 + 1; i<nframe; ++i){
if (ecg[i] >= ilevel){
track0[j] = i;
nbeat = nbeat + 1;
break;
}
if (i == nframe - 1) break;
}
if (track0[j]<0){
nbeat = nbeat - 1;
break;
}
if (track0[j] == 0) break;
for (i = 0; i <= j - 1; ++i){
beat = beat + (float)(track0[i + 1] - track0[i]);
}
beat = beat / (float)(j + 1);
}
tbeat = beat / fs * 1000;
/*for(i=0;i<nbeat;++i){
rr=ecg[track0[i]];
for(j>track0[i]-(int)(0.01*PRF2);j<track0[i]+(int)(0.01*PRF2);++j){
if(j>=0 && j<nframe && fabs(ecg[j])>rr){
rr=fabs(ecg[j]);
icnt=j;
}
}
track0[i]=j;
}*/
for (i = 0; i<nbeat; ++i){
printf("track0(%d) = %d\n", i, track0[i]);
}
//nbeat=2;
//track0[0]=0; track0[1]=nframe;
/* *************************************************************************
Setting Parameters
********************************************************************** */
ifrm1 = 0;
ifrm2 = nframe;
if (nbeat == 0){
ifrm1 = 0;
ifrm2 = nframe;
}
if (nbeat>0){
ifrm1 = track0[0] - (int)(0.2*PRF2);
if (ifrm1<0) ifrm1 = 0;
ifrm2 = nframe;
}
printf("ifrm1, ifrm2 = %d, %d\n", ifrm1, ifrm2);
if (ifrm1<0 || ifrm2>nframe || ifrm1>ifrm2) exit(0);
/* *************************************************************************
Display B-mode Image
********************************************************************** */
rmaxz = 0.0;
for (i = 0;i < nposi;++i){
for (j = 0; j < ncount; ++j) {
dmy[j] = pow((float)raw_dat[ihof[i][track0[0]] + j + 4], 2);
}
wint2s(wary, lp2s);
ndmy = ncount;
lpfs(wary, dmy, sgn, lp2s, ndmy);
for (j = 0; j<ncount; ++j){
sgn[j] = sqrt(sgn[j]);
if (fabs(sgn[j])>rmaxz) rmaxz = fabs(sgn[j]);
}
}
rwb = dadd / 8.;
nwb = (int)rwb;
/* 直交検波 */
for (i = 0; i<nposi; ++i){
for (j = 0; j<ncount; ++j){
dmy[j] = pow((float)raw_dat[ihof[i][track0[0]] + j + 1], 2);
}
wint2s(wary, lp2s);
ndmy = ncount;
lpfs(wary, dmy, sgn, lp2s, ndmy);
for (j = 0; j<ncount; ++j) sgn[j] = sqrt(sgn[j]);
for (j = 0; j<ncount; ++j){
ic = 49 + (int)(100.*(20.*log10((sgn[j] + 1e-4) / rmaxz) + B_gain) / B_gain);
if (ic<49) ic = 49;
if (ic>148) ic = 148;
ic = (ic - 49) * 255 / 99;
gscolor = gscol256(ic, ic, ic);
for (k = 0;k < (int)(izoom_B / (mpp / deltad) + 1);++k) {
gsline(Bx_ini + (int)(izoom_B*rwb*i),
By_ini + (int)(izoom_B*j*deltad / mpp) + k,
Bx_ini + (int)(izoom_B*rwb*(i + 1)),
By_ini + (int)(izoom_B*j*deltad / mpp) + k,
gscolor);
}
}
}
gscolor = gscol256(0, 0, 0);
gsline(Bx_ini - 15, By_ini, Bx_ini - 15,
By_ini + (int)(izoom_B*ncount*deltad / mpp), gscolor);
for (i = 0;i<ncount;i = i + (int)(5 * mpp / deltad)) {
gsline(Bx_ini - 15, By_ini + (int)(izoom_B*i*deltad / mpp),
Bx_ini - 10, By_ini + (int)(izoom_B*i*deltad / mpp), gscolor);
}
for (i = 0;i<ncount / 5;i = i + (int)(5 * mpp / deltad)) {
gsline(Bx_ini - 15, By_ini + (int)(5.*izoom_B*i*deltad / mpp),
Bx_ini - 5, By_ini + (int)(5.*izoom_B*i*deltad / mpp), gscolor);
}
gsline(Bx_ini, By_ini - 15, Bx_ini + (int)(izoom_B*rwb*nposi), By_ini - 15, gscolor);
for (i = 0;i<(int)(rwb*nposi / 10.) + 1;++i) {
gsline(Bx_ini + izoom_B * 10 * i, By_ini - 15,
Bx_ini + izoom_B * 10 * i, By_ini - 5, gscolor);
}
sprintf(buf, "%4d %56s", iseq, fall);
ptext(10, By_ini - 65, strlen(buf), buf);
sprintf(buf, "age:%2d BP: %3d/%3d", iage, ip_s, ip_d);
ptext(10, By_ini - 50, strlen(buf), buf);
sprintf(buf, "mkdirhier ./images/%s", fname);
//system(buf);
/*sprintf(buf,"import -window 0x%x -crop %dx%d+%d+%d -silent ./images/%s/Bmode.gif",
wid,(int)(izoom_B*rwb*nposi),
(int)(izoom_B*ncount*deltad/mpp),
Bx_ini,wwB-By_ini+60,fname);*/
sprintf(buf, "import -window 0x%x -silent ./images/%s/Bmode.gif", wid, fname);
//system(buf);
/* *************************************************************************
Setting Tracking Position by Tracing B-mode (itrace=1)
********************************************************************** */
if (itrace == 1){
gscolor = gscol256(0,0,255);
for (i = 0; i<nposi; ++i) ipeakflg[i] = 0;
cursorpos(0, 0);
trackingpoint_top();
cursorpos(0, 0);
for (i = 0; i<nposi0; ++i) ipeakflg[i] = 0;
trackingpoint_bot();
///* Save Peak Position */
//fin = fopen("./tmp/peaks", "w");
//for (i = 0; i<nposi; ++i)
// fprintf(fin, "%d %d %d %d\n", ipeak11[i], ipeak22[i], ipeak33[i], ipeak44[i]);
//fclose(fin);
}
if (itrace == 0){
sprintf(buf, "./tmp/peaks.%d", iseq1);
fin = fopen(buf, "r");
for (i = 0; i<nposi; ++i){
fscanf(fin, "%d %d %d %d", &ip1, &ip2, &ip3, &ip4);
ipeak11[i] = ip1; ipeak22[i] = ip2; ipeak33[i] = ip3; ipeak44[i] = ip4;
}
fclose(fin);
}
if (DispTr == 1){
gscolor = gscol256(255,0,0);
for (i = 0; i<nposi; ++i){
gscolor = gscol256(0,0,0);
for (k = 0; k<(int)(izoom_B / (mpp / deltad) + 1); ++k)
gsline(Bx_ini + (int)(izoom_B*rwb*i),
By_ini - (int)(izoom_B*ipeak33[i] * deltad / mpp) - k,
Bx_ini + (int)(izoom_B*rwb*(i + 1)),
By_ini - (int)(izoom_B*ipeak33[i] * deltad / mpp) - k, gscolor);
}
}
for (i = 0; i<nposi; ++i){
if (ipeak44[i] - ipeak33[i]<4 * idltx2) ipeak44[i] = ipeak33[i] + 4 * idltx2;
}
Sleep(1);
gscolor = gscol256(0,0,0);
if (ipause == 1){
printf("Input 0 if ready to go next.\n");
scanf("%d", &iflg);
}
/* *************************************************************************
Clear Window
********************************************************************** */
eraseg();
/* *************************************************************************
Estimation of Displacements
********************************************************************** */
ifrm1 = track0[anabt]; ifrm2 = track0[anabt + 2]; dsh = 1;
for (i = 0; i<nDFT; ++i) han_w[i] = 0.5 - 0.5*cos(rad*i / (float)nDFT);
sprintf(buf, "mkdirhier ./images/%s/Mmode", fname);
//system(buf);
//sprintf(buf, "./ddmax_%s.dat", fname);
sprintf(buf, "./ddmax.dat", fname);
fin = fopen(buf, "w");
for (iiposi = 0; iiposi<nposi; ++iiposi){
ddmax = 0.0;
eraseg();
sprintf(buf, "%4d %56s", iseq, fall);
ptext(100, wwB + 10, strlen(buf), buf);
sprintf(buf, "age:%2d, BP: %3d/%3d, vel. max: %5.1f mm/s, dd max: %6.1f um, beam position: %2d",
iage, ip_s, ip_d, 1e3*rryy, 1e6*rmaxdh, iiposi + 1);
ptext(100, wwB + 22, strlen(buf), buf);
if (aux_flg == 0)
sprintf(buf, "max. & bias of ECG: %7.1f, %7.1f, max. PCG: %7.1f",
ecgmax, ecgbias / PRF2, pcgmax);
if (aux_flg == 1)
sprintf(buf, "max & bias of ECG: %7.1f, %7.1f, max. PULSE: %7.1f",
ecgmax, ecgbias / PRF2, pcgmax);
ptext(100, wwB + 34, strlen(buf), buf);
/* *************************************************************************
Setting Tracking Position and Number of Tracking Points
********************************************************************** */
ndep[iiposi] = 2;
/* *************************************************************************
Display M-mode Image
********************************************************************** */
lp2s = (int)(2.0 / f0*fs);
Mx_ini = 50; My_ini = 800;
ry0 = nframe / 200.;
iy0 = (int)ry0;
if (iy0<1) iy0 = 1;
for (i = 0; i<200; ++i){
for (j = 0; j<ncount; ++j)
dmy[j] = pow((float)raw_dat[ihof[iiposi][(int)(ry0*i)] + j], 2);
wint2s(wary, lp2s);
ndmy = ncount;
lpfs(wary, dmy, sgn, lp2s, ndmy);
for (j = 0; j<ncount; ++j)
sgn[j] = sqrt(sgn[j]);
for (j = 0; j<ncount; j = j + (int)(mpp / deltad / izoom_M)){
ic = 49 + (int)(100.*((20.*log10(sgn[j] / rmaxz) + B_gain) / B_gain));
if (ic<49) ic = 49;
if (ic>148) ic = 148;
ic = (ic - 49) * 255 / 99;
gscolor = gscol256(ic, ic, ic);
for (k = -1; k <= 1; ++k)
gsline(Mx_ini + (int)(izoom_M*j*deltad / mpp), My_ini - 3 * i - k,
Mx_ini + (int)(izoom_M*j*deltad / mpp), My_ini - 3 * (i + 1) - k, gscolor);
}
}
gscolor = gscol256(0, 0, 0);
gsline(Mx_ini, My_ini + 15,
Mx_ini + (int)(izoom_M*ncount*deltad / mpp), My_ini + 15, gscolor);
for (i = 0; i < ncount; i = i + (int)(5 * mpp / deltad)) {
gsline(Mx_ini + (int)(izoom_M*i*deltad / mpp), My_ini + 15,
Mx_ini + (int)(izoom_M*i*deltad / mpp), My_ini + 10, gscolor);
}
for (i = 0; i < ncount / 5; i = i + (int)(5 * mpp / deltad)) {
gsline(Mx_ini + (int)(5.*izoom_M*i*deltad / mpp), My_ini + 15,
Mx_ini + (int)(5.*izoom_M*i*deltad / mpp), My_ini + 5, gscolor);
}
gsline(Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 10, My_ini,
Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 10, My_ini - 600, gscolor);
for (i = 0; i < (int)(2.*nframe / PRF2) + 1; ++i) {
gsline(Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 10,
My_ini - (int)(300.*PRF2 / nframe*i),
Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 5,
My_ini - (int)(300.*PRF2 / nframe*i), gscolor);
}
/* *************************************************************************
Frame for Displaying Signals
********************************************************************** */
for (i = 0; i<4; ++i){
gsrect(Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 20 + 100 * i,
My_ini + 15,
Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 110 + 100 * i,
My_ini - 615, RGB(255, 255, 255), RGB(0, 0, 0));
}
for (i = 0; i<4; ++i){
for (j = 0; j<2; ++j){
for (k = 0; k<3; ++k){
gsline(Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 30 + 35 * k + 100 * i,
My_ini + 15 - 622 * j,
Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 30 + 35 * k + 100 * i,
My_ini + 15 - 8 - 622 * j, gscolor);
}
}
}
for (i = 0; i<4; ++i){
gsline_d(Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 20 + 45 + 100 * i, My_ini,
Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 20 + 45 + 100 * i,
My_ini - 600, gscolor);
}
for (i = 0; i<4; ++i){
for (j = 0; j<2; ++j){
for (k = 0; k<(int)(2.*(ifrm2 - ifrm1) / PRF2) + 1; ++k){
gsline(Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 20 + 100 * i + 82 * j,
My_ini - (int)(300.*PRF2 / (ifrm2 - ifrm1)*k),
Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 20 + 8 + 100 * i + 82 * j,
My_ini - (int)(300.*PRF2 / (ifrm2 - ifrm1)*k), gscolor);
}
}
}
/* *************************************************************************
Display ECG
********************************************************************** */
ry0 = 600. / nframe; rx0 = 35. / ecgmax;
for (i = 0; i<nframe - 2; ++i){
nx0 = (int)(izoom_M*ncount*deltad / mpp) + 20 + 45;
gsline(Mx_ini + nx0 - (int)(rx0*ecg[i]),
My_ini - (int)(ry0*i),
Mx_ini + nx0 - (int)(rx0*ecg[i + 1]),
My_ini - (int)(ry0*(i + 1)), gscolor);
}
/* *************************************************************************
Display Timing of R-wave
********************************************************************** */
if (nbeat != 0){
nbeat2 = nbeat;
for (i = 0; i<nbeat; ++i){
if (track0[i]>0){
nbeat1 = i;
break;
}
}
for (i = 0; i<nbeat; ++i){
if (track0[i]>nframe){
nbeat2 = i - 1;
break;
}
}
if (nbeat1 == 0) nbeat1 = 0;
if (nbeat2 == 0) nbeat2 = 0;
gscolor = gscol256(255, 0, 0);
nx0 = (int)(izoom_M*ncount*deltad / mpp);
for (i = nbeat1; i<nbeat2; ++i){
gsline_d(Mx_ini, My_ini - (int)(ry0*track0[i]),
Mx_ini + nx0, My_ini - (int)(ry0*track0[i]), gscolor);
}
nx0 = (int)(izoom_M*ncount*deltad / mpp) + 30;
for (i = 0; i<4; ++i){
for (j = nbeat1; j<nbeat2; ++j){
gsline_d(Mx_ini + nx0 + 100 * i, My_ini - (int)(ry0*track0[j]),
Mx_ini + nx0 + 70 + 100 * i, My_ini - (int)(ry0*track0[j]), gscolor);
}
}
gscolor = gscol256(0, 0, 0);
}
/* *************************************************************************
Display PCG
********************************************************************** */
/*ry0=600./nframe; rx0=35./pcgmax;
for(i=0;i<nframe-1;++i){
nx0=(int)(izoom_M*ncount*deltad/mpp)+20+145;
gsline(Mx_ini+nx0-(int)(rx0*pcg[i]),
My_ini+(int)(ry0*i),
Mx_ini+nx0-(int)(rx0*pcg[i+1]),
My_ini+(int)(ry0*(i+1)));
}*/
/* *************************************************************************
Start of Loop for Frame for Displacement Estimation
********************************************************************** */
for (idcnt = 0; idcnt<ndep[iiposi]; ++idcnt){
disp[idcnt][ifrm1] = 0.0;
dd[ifrm1] = 0.0;
}
ip1 = 0; ip2 = ncount;
for (ifrm = ifrm1; ifrm<ifrm2; ++ifrm){
/* Quadrature Demodulation */
czeros = cmplx(0.0, 0.0);
for (i = -1; i<2; ++i){
if (iiposi + i>-1 && iiposi + i<nposi){
/* For Previous Frame */
if (ifrm == ifrm1){
for (j = ip1; j<ip2; ++j){
qdr_p[j][i + 1] = 0.0; qdi_p[j][i + 1] = 0.0; icnt = 0;
for (k = -nDFT / 2; k < nDFT / 2; ++k) {
if (j + k >= 0 && j + k<ncount){
qdr_p[j][i + 1] = qdr_p[j][i + 1] + han_w[k + nDFT / 2] *
cos(nfrq*rad*(k + nDFT / 2) / (float)nDFT)*
(float)raw_dat[ihof[iiposi + i][ifrm] + j + k];
qdi_p[j][i + 1] = qdi_p[j][i + 1] - han_w[k + nDFT / 2] *
sin(nfrq*rad*(k + nDFT / 2) / (float)nDFT)*
(float)raw_dat[ihof[iiposi + i][ifrm] + j + k];
++icnt;
}
}
qdr_p[j][i + 1] = qdr_p[j][i + 1] / (float)icnt;
qdi_p[j][i + 1] = qdi_p[j][i + 1] / (float)icnt;
}
}
/* For Post-Frame */
for (j = ip1; j<ip2; ++j){
qdr_f[j][i + 1] = 0.0; qdi_f[j][i + 1] = 0.0; icnt = 0;
for (k = -nDFT / 2; k<nDFT / 2; ++k){
if (j + k >= 0 && j + k<ncount){
qdr_f[j][i + 1] = qdr_f[j][i + 1] + han_w[k + nDFT / 2] *
cos(nfrq*rad*(k + nDFT / 2) / (float)nDFT)*
(float)raw_dat[ihof[iiposi + i][ifrm + 1] + j + k];
qdi_f[j][i + 1] = qdi_f[j][i + 1] - han_w[k + nDFT / 2] *
sin(nfrq*rad*(k + nDFT / 2) / (float)nDFT)*
(float)raw_dat[ihof[iiposi + i][ifrm + 1] + j + k];
++icnt;
}
}
qdr_f[j][i + 1] = qdr_f[j][i + 1] / (float)icnt;
qdi_f[j][i + 1] = qdi_f[j][i + 1] / (float)icnt;
}
}
}
/* Start of Loop of Depth for Displacement Estimation */
for (idcnt = 0; idcnt<ndep[iiposi]; ++idcnt){
if (ifrm == ifrm1){
if (idcnt == 0) peak0[idcnt] = ipeak11[iiposi];
if (idcnt == 1) peak0[idcnt] = ipeak22[iiposi];
}
xpeak0[idcnt] = (float)peak0[idcnt];
// velocity estimation
crr0 = czeros; crr1 = czeros;
for (ispc = -nspc; ispc<nspc + 1; ++ispc){
if (iiposi + ispc >= 0 && iiposi + ispc<nposi){
for (i = 0; i <= 2 * idltx; ++i){
if (i + peak0[idcnt] >= 0 && i + peak0[idcnt]<ncount){
rr = qdr_p[i + peak0[idcnt]][ispc + 1];
ri = qdi_p[i + peak0[idcnt]][ispc + 1];
cdmyp = cmplx(rr, ri);
rr = qdr_f[i + peak0[idcnt]][ispc + 1];
ri = qdi_f[i + peak0[idcnt]][ispc + 1];
cdmy = cmplx(rr, ri);
ctemp = conjg(cdmyp);
crr0 = cadd(crr0, cmul(cdmy, ctemp));
}
}
}
}
vel_p[idcnt] = vel[idcnt];
if (ifrm == ifrm1) vel_p[idcnt] = 0.0;
vel[idcnt] = -0.5*c0*atan2(crr0.i, crr0.r) / rad / f0;
disp[idcnt][ifrm + 1] = disp[idcnt][ifrm] + vel[idcnt];
if (idcnt == 0){
xpeak0[idcnt] = ipeak11[iiposi] + disp[idcnt][ifrm + 1] / deltad;
}
if (idcnt == 1){
xpeak0[idcnt] = ipeak22[iiposi] + disp[idcnt][ifrm + 1] / deltad;
}
peak0[idcnt] = (int)xpeak0[idcnt];
}
dd[ifrm + 1] = disp[1][ifrm + 1] - disp[0][ifrm + 1];
ry0 = 600. / nframe;
for (idcnt = 0; idcnt<ndep[iiposi]; ++idcnt){
/* Display Tracked Position */
gscolor = gscol256(255, 0, 0);
if (ifrm >= ifrm1 && ifrm<ifrm2)
gsline(Mx_ini + (int)(izoom_M*peak0[idcnt] * deltad / mpp),
My_ini - (int)(ry0*ifrm),
Mx_ini + (int)(izoom_M*peak0[idcnt] * deltad / mpp),
My_ini - (int)(ry0*(ifrm + 1)), gscolor);
gscolor = gscol256(255, 255, 255);
gsline(Mx_ini + 1 + (int)(izoom_M*peak0[idcnt] * deltad / mpp),
My_ini - (int)(ry0*ifrm),
Mx_ini + 1 + (int)(izoom_M*peak0[idcnt] * deltad / mpp),
My_ini - (int)(ry0*(ifrm + 1)), gscolor);
gscolor = gscol256(0, 0, 0);
/* Display Velocity */
rx0 = 35. / rryy;
if (idcnt == 0) nx0 = (int)(izoom_M*ncount*deltad / mpp) + 20 + 145;
if (idcnt == 1) nx0 = (int)(izoom_M*ncount*deltad / mpp) + 20 + 245;
if (ifrm > ifrm1 && ifrm < ifrm2 - 1) {
gsline(Mx_ini + nx0 + (int)(rx0*vel_p[idcnt] * PRF2),
My_ini - (int)(ry0*ifrm),
Mx_ini + nx0 + (int)(rx0*vel[idcnt] * PRF2),
My_ini - (int)(ry0*(ifrm + 1)), gscolor);
}
}
/* Display Change in Diameter */
rx0 = 35. / rmaxdh;
nx0 = (int)(izoom_M*ncount*deltad / mpp) + 20 + 345;
if (ifrm >= ifrm1 && ifrm<ifrm2 - 1){
gsline(Mx_ini + nx0 - (int)(rx0*dd[ifrm]),
My_ini - (int)(ry0*ifrm),
Mx_ini + nx0 - (int)(rx0*dd[ifrm + 1]),
My_ini - (int)(ry0*(ifrm + 1)), gscolor);
}
for (i = 0; i<3; ++i){
for (j = 0; j<ncount; ++j){
qdr_p[j][i] = qdr_f[j][i]; qdi_p[j][i] = qdi_f[j][i];
}
}
if (ddmax>dd[ifrm + 1])
ddmax = dd[ifrm + 1];
} // end of loop of frame
sprintf(buf, "import -window 0x%x -silent ./images/%s/Mmode/pos%d.gif", wid, fname, iiposi);
//system(buf);
fprintf(fin, "%d %f\n", iiposi, ddmax*1e+6);
} // end of loop of beam position
fclose(fin);
/* *************************************************************************
Terminate Program
********************************************************************** */
free(raw_dat); free(sgn); free(dmy);
end();
printf("INPUT 0 for termination of program.\n");
scanf("%d", &iflg);
printf("*******************************\n");
printf("* normally terminated *\n");
printf("*******************************\n");
}
float rx_est_timing(COMP rx_symbols[],
COMP rx_filt[NC+1][P+1],
COMP rx_baseband[NC+1][M+M/P],
COMP rx_filter_mem_timing[NC+1][NT*P],
float env[],
COMP rx_baseband_mem_timing[NC+1][NFILTERTIMING],
int nin)
{
int c,i,j,k;
int adjust, s;
COMP x, phase, freq;
float rx_timing;
/*
nin adjust
--------------------------------
120 -1 (one less rate P sample)
160 0 (nominal)
200 1 (one more rate P sample)
*/
adjust = P - nin*P/M;
/* update buffer of NT rate P filtered symbols */
for(c=0; c<NC+1; c++)
for(i=0,j=P-adjust; i<(NT-1)*P+adjust; i++,j++)
rx_filter_mem_timing[c][i] = rx_filter_mem_timing[c][j];
for(c=0; c<NC+1; c++)
for(i=(NT-1)*P+adjust,j=0; i<NT*P; i++,j++)
rx_filter_mem_timing[c][i] = rx_filt[c][j];
/* sum envelopes of all carriers */
for(i=0; i<NT*P; i++) {
env[i] = 0.0;
for(c=0; c<NC+1; c++)
env[i] += cabsolute(rx_filter_mem_timing[c][i]);
}
/* The envelope has a frequency component at the symbol rate. The
phase of this frequency component indicates the timing. So work
out single DFT at frequency 2*pi/P */
x.real = 0.0; x.imag = 0.0;
freq.real = cosf(2*PI/P);
freq.imag = sinf(2*PI/P);
phase.real = 1.0;
phase.imag = 0.0;
for(i=0; i<NT*P; i++) {
x = cadd(x, fcmult(env[i], phase));
phase = cmult(phase, freq);
}
/* Map phase to estimated optimum timing instant at rate M. The
M/4 part was adjusted by experiment, I know not why.... */
rx_timing = atan2f(x.imag, x.real)*M/(2*PI) + M/4;
if (rx_timing > M)
rx_timing -= M;
if (rx_timing < -M)
rx_timing += M;
/* rx_filt_mem_timing contains M + Nfilter + M samples of the
baseband signal at rate M this enables us to resample the
filtered rx symbol with M sample precision once we have
rx_timing */
for(c=0; c<NC+1; c++)
for(i=0,j=nin; i<NFILTERTIMING-nin; i++,j++)
rx_baseband_mem_timing[c][i] = rx_baseband_mem_timing[c][j];
for(c=0; c<NC+1; c++)
for(i=NFILTERTIMING-nin,j=0; i<NFILTERTIMING; i++,j++)
rx_baseband_mem_timing[c][i] = rx_baseband[c][j];
/* rx filter to get symbol for each carrier at estimated optimum
timing instant. We use rate M filter memory to get fine timing
resolution. */
s = round(rx_timing) + M;
for(c=0; c<NC+1; c++) {
rx_symbols[c].real = 0.0;
rx_symbols[c].imag = 0.0;
for(k=s,j=0; k<s+NFILTER; k++,j++)
rx_symbols[c] = cadd(rx_symbols[c], fcmult(gt_alpha5_root[j], rx_baseband_mem_timing[c][k]));
}
return rx_timing;
}
/**
Calculate total error. If g0 is 0, use st->g, otherwith use g0 to figure out g, a T.
*/
static double servo_calc_do(SERVO_CALC_T *st, double g0){
dmat *nu=st->nu;
if(!st->Hol){
st->Hol=cnew(nu->nx,1);
}
/*Compute Hol with the first integrator and gain.*/
if(g0>EPS){
st->g=g0;
}
cadd(&st->Hol, 0, st->Hsys, st->g);
double g2=1;/*additional g to multiply. !=1 if g0 is nonzero and type is 2.*/
if(st->type==2){//type II controller
ccwm(st->Hol, st->Hint);/*multiply the second integrator*/
if(fabs(g0)>EPS){/*figure out a, T from new g0*/
double margin, fcross;
margin=phase_at_gain(&fcross, nu, st->Hol, 1);/*see how much phase lead is needed*/
double phineed=st->pmargin-margin;
double a,T;
if(phineed*2.2>M_PI){/*lead filter is not suitable*/
a=1;
T=0;
}else{
a=(1-sin(phineed))/(1+sin(phineed));
double f0=fcross*sqrt(a);
T=1./(2.*M_PI*f0);
}
/* Hlead is multiplied by sqrt(a) so it has unit gain at cross over frequency*/
g2=sqrt(a);
st->g=g0*g2;
st->a=a;
st->T=T;
//info("g0=%g, g2=%g, phineed=%.1f\n", g0, g2, phineed*180/M_PI);
}
double a=st->a;
double T=st->T;
for(int i=0; i<nu->nx; i++){
dcomplex Hlead=(1+T*st->s->p[i])/(1+a*T*st->s->p[i])*g2;
st->Hol->p[i]*=Hlead;
}
}
double res_sig=0;
double sum_n=0, sum_1=0;
dmat *psd=st->psd;
for(int i=0; i<nu->nx; i++){
dcomplex Hol=st->Hol->p[i];
dcomplex Hrej=1./(1.+Hol);
//The gain never reach below -50dB
res_sig+=psd->p[i]*creal(Hrej*conj(Hrej)+1e-5)*nu->p[i];
if(st->nu->p[i]<st->fny){
//compute noise prop only within nyqust frequency
dcomplex Hcl=Hol*Hrej;
dcomplex Hwfs=st->Hwfs->p[i];
dcomplex Hn=Hcl/Hwfs;
sum_n+=creal(Hn*conj(Hn))*nu->p[i];
sum_1+=nu->p[i];
}
}
double dlognu=(log(nu->p[nu->nx-1])-log(nu->p[0]))/(nu->nx-1);
res_sig*=dlognu;
st->res_sig=res_sig;
st->gain_n=sum_n/sum_1;
if(!servo_isstable(nu, st->Hol)){
st->gain_n=100;
/*put a high penalty to drive down the gain*/
st->res_n=10*(1+g0)*(st->var_sig+st->sigma2n);
/*warning2("Unstable: g0=%g, g2=%g, res_sig=%g, res_n=%g, tot=%g, gain_n=%g sigma2n=%g\n",
g0, g2, res_sig, st->res_n, st->res_n+res_sig, st->gain_n, st->sigma2n);*/
}else{
if(st->gain_n>1){
st->gain_n=pow(st->gain_n,3);/*a fudge factor to increase the penalty*/
}
st->res_n=st->sigma2n*st->gain_n;
/*info2(" Stable: g0=%g, g2=%g, res_sig=%g, res_n=%g, tot=%g, gain_n=%g sigma2n=%g\n",
g0, g2, res_sig, st->res_n, st->res_n+res_sig, st->gain_n, st->sigma2n);*/
}
return res_sig+st->res_n;
}
