int computeHead(int variableNum) {
//double data[JOINT_NUM][JOINT_DATA_NUM], double pos_data[POS_JOINT_NUM][POS_JOINT_DATA_NUM], int variableNum) {
int curFrame = findFrame(0);
// compute hand loc
double heap_pos[3];
double head_pos[3];
double head_up[3];
double he_fs = 0; // head from sensor
double hp_fs = 0; // heap from sensor
//double head_ori[9];
/*for (int i=0;i<9;i++) {
head_ori[i] = prevFrame[curFrame][HEAD_JOINT_NUM][i];
}*/
for (int i=0;i<3;i++) {
double ltemp = prevFrame[curFrame][7][i+9];
double rtemp = prevFrame[curFrame][9][i+9];
heap_pos[i] = (ltemp+rtemp)/2;
head_pos[i] = prevFrame[curFrame][HEAD_JOINT_NUM][i+9];
head_up[i] = heap_pos[i];
if (i==1) { // y val
head_up[i] = head_pos[i];
}
if (i!=1) { // except y val
he_fs += (head_pos[i]*head_pos[i]);
hp_fs += (heap_pos[i]*heap_pos[i]);
}
//printf("%d: head: %.1f heap: %.1f up: %.1f\n", i+1, head_pos[i], heap_pos[i], head_up[i]);
}
double v1[3]; // heap to head
double v2[3]; // heap to up
double v1_det = 0;
double v2_det = 0;
double dot = 0;
for (int i=0;i<3;i++) {
v1[i] = head_pos[i] - heap_pos[i];
v2[i] = head_up[i] - heap_pos[i];
dot += (v1[i]*v2[i]);
v1_det += (v1[i]*v1[i]);
v2_det += (v2[i]*v2[i]);
}
v1_det = sqrt(v1_det);
v2_det = sqrt(v2_det);
double ang = acos(dot / (v1_det * v2_det));
ang = ang * 180 / 3.14159265; // to degree
if (he_fs > hp_fs) { // if heap is closer
ang = -ang;
}
if (ang>80) {
// printf("computeHead angle not right.. %.1f \n", ang); exit(1);
}
// fprintf(pRecFile, "%.1f,", ang);
featureValues.push_back(ang);
variableNum++;
/*double* headLoc = computeLocalHandLoc(head_ori, head_pos, heap_pos);
for (int i=0;i<3;i++) {
fprintf(pRecFile, "%.7f,", headLoc[i]);
variableNum++;
}*/
if (DEBUG_numFeature) printf("HEAD angle feature: %d\n", variableNum);
return variableNum;
} // end computeHead
//2点間の距離を計算
double Vector2D::Distance(const Vector2D &vec) const
{
return sqrt((vec.x * vec.x) + (vec.y * vec.y));
}
double dist(pt_t a, pt_t b)
{
return sqrt(SQR(a.x - b.x) + SQR(a.y - b.y));
}
void
F77_FUNC(slartg,SLARTG)(float *f,
float *g,
float *cs,
float *sn,
float *r)
{
float minval,safemin, safemin2, safemx2, eps;
float f1,g1,f1a,g1a,scale;
int i,n,count;
eps = GMX_FLOAT_EPS;
minval = GMX_FLOAT_MIN;
safemin = minval*(1.0+eps);
n = 0.5*log( safemin/eps ) / log(2);
safemin2 = pow(2,n);
safemx2 = 1.0 / safemin2;
if(fabs(*g)<GMX_FLOAT_MIN) {
*cs = 1.0;
*sn = 0.0;
*r = *f;
} else if (fabs(*f)<GMX_FLOAT_MIN) {
*cs = 0.0;
*sn = 1.0;
*r = *g;
} else {
f1 = *f;
g1 = *g;
f1a = fabs(f1);
g1a = fabs(g1);
scale = (f1a > g1a) ? f1a : g1a;
if(scale >= safemx2) {
count = 0;
while(scale >= safemx2) {
count++;
f1 *= safemin2;
g1 *= safemin2;
f1a = fabs(f1);
g1a = fabs(g1);
scale = (f1a > g1a) ? f1a : g1a;
}
*r = sqrt(f1*f1 + g1*g1);
*cs = f1 / *r;
*sn = g1 / *r;
for(i=0;i<count;i++)
*r *= safemx2;
} else if (scale<=safemin2) {
count = 0;
while(scale <= safemin2) {
count++;
f1 *= safemx2;
g1 *= safemx2;
f1a = fabs(f1);
g1a = fabs(g1);
scale = (f1a > g1a) ? f1a : g1a;
}
*r = sqrt(f1*f1 + g1*g1);
*cs = f1 / *r;
*sn = g1 / *r;
for(i=0;i<count;i++)
*r *= safemin2;
} else {
*r = sqrt(f1*f1 + g1*g1);
*cs = f1 / *r;
*sn = g1 / *r;
}
if(fabs(*f)>fabs(*g) && *cs<0.0) {
*cs *= -1.0;
*sn *= -1.0;
*r *= -1.0;
}
}
return;
}
pulsesequence()
{
/* DECLARE AND LOAD VARIABLES */
void makeHHdec(), makeCdec(); /* utility functions */
char f1180[MAXSTR], /* Flag to start t1 @ halfdwell */
mag_flg[MAXSTR], /* magic-angle coherence transfer gradients */
C13refoc[MAXSTR], /* C13 sech/tanh pulse in middle of t1*/
NH2only[MAXSTR], /* spectrum of only NH2 groups */
T1[MAXSTR], /* insert T1 relaxation delay */
T1rho[MAXSTR], /* insert T1rho relaxation delay */
T2[MAXSTR], /* insert T2 relaxation delay */
TROSY[MAXSTR], /* do TROSY on N15 and H1 */
Hdecflg[MAXSTR], /* HH-h**o decoupling flag */
Cdecflg[MAXSTR]; /* low power C-13 decoupling flag */
int icosel, /* used to get n and p type */
ihh=1, /* used in HH decouling to improve water suppression */
t1_counter, /* used for states tppi in t1 */
rTnum, /* number of relaxation times, relaxT */
rTcounter; /* to obtain maximum relaxT, ie relaxTmax */
double tau1, /* t1 delay */
lambda = 0.91/(4.0*getval("JNH")), /* 1/4J H1 evolution delay */
tNH = 1.0/(4.0*getval("JNH")), /* 1/4J N15 evolution delay */
relaxT = getval("relaxT"), /* total relaxation time */
rTarray[1000], /* to obtain maximum relaxT, ie relaxTmax */
maxrelaxT = getval("maxrelaxT"), /* maximum relaxT in all exps */
ncyc, /* number of pulsed cycles in relaxT */
pwr_dly, /* power delay */
/* the sech/tanh pulse is automatically calculated by the macro "proteincal", */
/* and is called directly from your shapelib. */
pwClvl = getval("pwClvl"), /* coarse power for C13 pulse */
pwC = getval("pwC"), /* C13 90 degree pulse length at pwClvl */
rf0, /* maximum fine power when using pwC pulses */
rfst, /* fine power for the stCall pulse */
compH = getval("compH"), /* adjustment for H1 amplifier compression */
compN = getval("compN"), /* adjustment for N15 amplifier compression */
compC = getval("compC"), /* adjustment for C13 amplifier compression */
calH = getval("calH"), /* multiplier on a pw pulse for H1 calibration */
tpwrsf = getval("tpwrsf"), /* fine power adustment for soft pulse */
pwHs = getval("pwHs"), /* H1 90 degree pulse length at tpwrs */
pwHH = 0.0, /* pwHH = pwHs for HH h**o-decoupling */
tpwrs, /* power for the pwHs ("H2Osinc") pulse */
pwNlvl = getval("pwNlvl"), /* power for N15 pulses */
pwN = getval("pwN"), /* N15 90 degree pulse length at pwNlvl */
calN = getval("calN"), /* multiplier on a pwN pulse for calibration */
slNlvl, /* power for N15 spin lock */
slNrf = 1500.0, /* RF field in Hz for N15 spin lock at 600 MHz */
sw1 = getval("sw1"),
gt1 = getval("gt1"), /* coherence pathway gradients */
gzcal = getval("gzcal"), /* dac to G/cm conversion */
gzlvl1 = getval("gzlvl1"),
gzlvl2 = getval("gzlvl2"),
BPpwrlimits, /* =0 for no limit, =1 for limit */
gt0 = getval("gt0"), /* other gradients */
gt3 = getval("gt3"),
gt4 = getval("gt4"),
gt5 = getval("gt5"),
gstab = getval("gstab"),
gzlvl0 = getval("gzlvl0"),
gzlvl3 = getval("gzlvl3"),
gzlvl4 = getval("gzlvl4"),
gzlvl5 = getval("gzlvl5");
P_getreal(GLOBAL,"BPpwrlimits",&BPpwrlimits,1);
getstr("f1180",f1180);
getstr("mag_flg",mag_flg);
getstr("C13refoc",C13refoc);
getstr("NH2only",NH2only);
getstr("T1",T1);
getstr("T1rho",T1rho);
getstr("T2",T2);
getstr("TROSY",TROSY);
getstr("Hdecflg", Hdecflg);
getstr("Cdecflg", Cdecflg);
/* LOAD PHASE TABLE */
settable(t3,2,phi3);
settable(t4,1,phx);
if (TROSY[A]=='y')
{settable(t1,1,ph_x);
settable(t9,1,phx);
settable(t10,1,phy);
settable(t11,1,phx);
settable(t12,2,recT);}
else
{settable(t1,1,phx);
settable(t9,8,phi9);
settable(t10,1,phx);
settable(t11,1,phy);
settable(t12,4,rec);}
/* INITIALIZE VARIABLES */
/* maximum fine power for pwC pulses (and initialize rfst) */
rf0 = 4095.0; rfst=0.0;
/* 180 degree adiabatic C13 pulse from 0 to 200 ppm */
if (C13refoc[A]=='y')
{rfst = (compC*4095.0*pwC*4000.0*sqrt((30.0*sfrq/600.0+7.0)/0.35));
rfst = (int) (rfst + 0.5);
if ( 1.0/(4000.0*sqrt((30.0*sfrq/600.0+7.0)/0.35)) < pwC )
{ text_error( " Not enough C13 RF. pwC must be %f usec or less.\n",
(1.0e6/(4000.0*sqrt((30.0*sfrq/600.0+7.0)/0.35))) ); psg_abort(1); }}
/* selective H20 one-lobe sinc pulse */
if(pwHs > 1e-6)
tpwrs = tpwr - 20.0*log10(pwHs/(compH*pw*1.69)); /* needs 1.69 times more */
else /* power than a square pulse */
tpwrs = 0.0;
tpwrs = (int) (tpwrs);
if (tpwrsf<4095.0) tpwrs = tpwrs + 6.0;
if (tpwrsf < 4095.0)
{
tpwrs = tpwrs + 6.0;
pwr_dly = POWER_DELAY + PWRF_DELAY;
}
else pwr_dly = POWER_DELAY;
/* power level for N15 spinlock (90 degree pulse length calculated first) */
slNlvl = 1/(4.0*slNrf*sfrq/600.0) ;
slNlvl = pwNlvl - 20.0*log10(slNlvl/(pwN*compN));
slNlvl = (int) (slNlvl + 0.5);
/* use 1/8J times for relaxation measurements of NH2 groups */
if ( (NH2only[A]=='y') && ((T1[A]=='y') || (T1rho[A]=='y') || (T2[A]=='y')) )
{ tNH = tNH/2.0; }
/* reset calH and calN for 2D if inadvertently left at 2.0 */
if (ni>1.0) {calH=1.0; calN=1.0;}
/* make shapes and set up parameters for HH h**o-decoupling */
if(Cdecflg[0] == 'y') makeCdec();
if(Hdecflg[0] == 'y') makeHHdec();
if(Hdecflg[0] != 'n')
{
pwHH = pwHs;
pwHs = 0.0;
}
/* CHECK VALIDITY OF PARAMETER RANGES */
if ((TROSY[A]=='y') && (gt1 < -2.0e-4 + pwHs + 1.0e-4 + 2.0*POWER_DELAY))
{ text_error( " gt1 is too small. Make gt1 equal to %f or more.\n",
(-2.0e-4 + pwHs + 1.0e-4 + 2.0*POWER_DELAY) ); psg_abort(1); }
if((dm[A] == 'y' || dm[B] == 'y' || dm[C] == 'y' ))
{ text_error("incorrect dec1 decoupler flags! Should be 'nnn' "); psg_abort(1); }
if((dm2[A] == 'y' || dm2[B] == 'y'))
{ text_error("incorrect dec2 decoupler flags! Should be 'nny' "); psg_abort(1); }
if( dpwr2 > 50 )
{ text_error("don't fry the probe, DPWR2 too large! "); psg_abort(1); }
if( pw > 50.0e-6 )
{ text_error("dont fry the probe, pw too high ! "); psg_abort(1); }
if( pwN > 100.0e-6 )
{ text_error("dont fry the probe, pwN too high ! "); psg_abort(1); }
/* RELAXATION TIMES AND FLAGS */
/* evaluate maximum relaxT, relaxTmax chosen by the user */
rTnum = getarray("relaxT", rTarray);
relaxTmax = rTarray[0];
for (rTcounter=1; rTcounter<rTnum; rTcounter++)
if (relaxTmax < rTarray[rTcounter]) relaxTmax = rTarray[rTcounter];
/* compare relaxTmax with maxrelaxT */
if (maxrelaxT > relaxTmax) relaxTmax = maxrelaxT;
if ( ((T1rho[A]=='y') || (T2[A]=='y')) && (relaxTmax > d1) )
{ text_error("Maximum relaxation time, relaxT, is greater than d1 ! "); psg_abort(1);}
if ( ((T1[A]=='y') && (T1rho[A]=='y')) || ((T1[A]=='y') && (T2[A]=='y')) ||
((T1rho[A]=='y') && (T2[A]=='y')) )
{ text_error("Choose only one relaxation measurement ! "); psg_abort(1); }
if ( ((T1[A]=='y') || (T1rho[A]=='y')) &&
((relaxT*100.0 - (int)(relaxT*100.0+1.0e-4)) > 1.0e-6) )
{ text_error("Relaxation time, relaxT, must be zero or multiple of 10msec"); psg_abort(1);}
if ( (T2[A]=='y') &&
(((relaxT+0.01)*50.0 - (int)((relaxT+0.01)*50.0+1.0e-4)) > 1.0e-6) )
{ text_error("Relaxation time, relaxT, must be odd multiple of 10msec"); psg_abort(1);}
if ( ((T1rho[A]=='y') || (T2[A]=='y')) && (relaxTmax > 0.25) && (ix==1) )
{ printf("WARNING, sample heating will result for relaxT>0.25sec"); }
if ( ((T1rho[A]=='y') || (T2[A]=='y')) && (relaxTmax > 0.5) )
{ text_error("relaxT greater than 0.5 seconds will heat sample"); psg_abort(1);}
if ( ((NH2only[A]=='y') || (T1[A]=='y') || (T1rho[A]=='y') || (T2[A]=='y'))
&& (TROSY[A]=='y') )
{ text_error("TROSY not implemented with NH2 spectrum, or relaxation exps."); psg_abort(1);}
if ((TROSY[A]=='y') && (dm2[C] == 'y'))
{ text_error("Choose either TROSY='n' or dm2='n' ! "); psg_abort(1); }
/* PHASES AND INCREMENTED TIMES */
/* Phase incrementation for hypercomplex 2D data, States-Haberkorn element */
if (TROSY[A]=='y')
{ if (phase1 == 2) icosel = -1;
else { tsadd(t4,2,4); tsadd(t10,2,4); icosel = +1; }
}
else { if (phase1 == 2) {tsadd(t10,2,4); icosel = +1;}
else icosel = -1;
}
if(Hdecflg[0] != 'n') ihh = icosel;
/* Set up f1180 */
tau1 = d2;
if((f1180[A] == 'y') && (ni > 1.0))
{ tau1 += ( 1.0 / (2.0*sw1) ); if(tau1 < 0.2e-6) tau1 = 0.0; }
tau1 = tau1/2.0;
/* Calculate modifications to phases for States-TPPI acquisition */
if( ix == 1) d2_init = d2;
t1_counter = (int) ( (d2-d2_init)*sw1 + 0.5 );
if(t1_counter % 2)
{ tsadd(t3,2,4); tsadd(t12,2,4); }
/* Correct inverted signals for NH2 only spectra */
if ((NH2only[A]=='y') && (T1[A]=='n') && (T1rho[A]=='n') && (T2[A]=='n'))
{ tsadd(t3,2,4); }
/* BEGIN PULSE SEQUENCE */
status(A);
obspower(tpwr);
decpower(pwClvl);
decpwrf(rf0);
dec2power(pwNlvl);
txphase(zero);
decphase(zero);
dec2phase(zero);
if(Hdecflg[0] != 'n')
{
delay(5.0e-5);
rgpulse(pw,zero,rof1,0.0);
rgpulse(pw,one,0.0,rof1);
zgradpulse(1.5*gzlvl0, 0.5e-3);
delay(5.0e-4);
rgpulse(pw,zero,rof1,0.0);
rgpulse(pw,one,0.0,rof1);
zgradpulse(-gzlvl0, 0.5e-3);
}
delay(d1);
/* xxxxxxxxxxxxxxxxx CONSTANT SAMPLE HEATING FROM N15 RF xxxxxxxxxxxxxxxxx */
if (T1rho[A]=='y')
{dec2power(slNlvl);
dec2rgpulse(relaxTmax-relaxT, zero, 0.0, 0.0);
dec2power(pwNlvl);}
if (T2[A]=='y')
{ncyc = 8.0*100.0*(relaxTmax - relaxT);
if (BPpwrlimits > 0.5)
{
dec2power(pwNlvl-3.0); /* reduce for probe protection */
pwN=pwN*compN*1.4;
}
if (ncyc > 0)
{initval(ncyc,v1);
loop(v1,v2);
delay(0.625e-3 - pwN);
dec2rgpulse(2*pwN, zero, 0.0, 0.0);
delay(0.625e-3 - pwN);
endloop(v2);}
if (BPpwrlimits > 0.5)
{
dec2power(pwNlvl); /* restore normal value */
pwN=getval("pwN");
}
}
/* xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx */
rcvroff();
if (TROSY[A]=='n')
dec2rgpulse(pwN, zero, 0.0, 0.0); /*destroy N15 magnetization*/
zgradpulse(gzlvl0, 0.5e-3);
delay(1.0e-4);
if (TROSY[A]=='n') dec2rgpulse(pwN, one, 0.0, 0.0);
zgradpulse(0.7*gzlvl0, 0.5e-3);
decpwrf(rfst);
txphase(t1);
delay(5.0e-4);
if ( dm3[B] == 'y' ) /* begins optional 2H decoupling */
{
lk_hold();
dec3rgpulse(1/dmf3,one,10.0e-6,2.0e-6);
dec3unblank();
dec3phase(zero);
delay(2.0e-6);
setstatus(DEC3ch, TRUE, 'w', FALSE, dmf3);
}
rgpulse(calH*pw,t1,0.0,0.0); /* 1H pulse excitation */
txphase(zero);
dec2phase(zero);
zgradpulse(gzlvl0, gt0);
delay(lambda - gt0 - pwHH);
if(Hdecflg[0] != 'n')
{
obspower(tpwrs);
if (tpwrsf<4095.0) obspwrf(tpwrsf);
shaped_pulse("H2Osinc", pwHH, two, 5.0e-5, 0.0);
obspower(tpwr);
if (tpwrsf<4095.0) obspwrf(4095.0);
sim3pulse(2.0*pw, 0.0, 2.0*pwN, zero, zero, zero, 0.0, 0.0);
obspower(tpwrs);
if (tpwrsf<4095.0) obspwrf(tpwrsf);
shaped_pulse("H2Osinc", pwHH, two, 5.0e-5, 0.0);
obspower(tpwr);
if (tpwrsf<4095.0) obspwrf(4095.0);
}
else
sim3pulse(2.0*pw, 0.0, 2.0*pwN, zero, zero, zero, 0.0, 0.0);
txphase(one);
zgradpulse(gzlvl0, gt0);
delay(lambda - gt0 - pwHH);
rgpulse(pw, one, 0.0, 0.0);
txphase(two);
obspower(tpwrs);
if (tpwrsf<4095.0) obspwrf(tpwrsf);
shaped_pulse("H2Osinc", pwHs, two, 5.0e-5, 0.0);
obspower(tpwr);
if (tpwrsf<4095.0) obspwrf(4095.0);
if (TROSY[A]=='y')
zgradpulse(ihh*gzlvl3, gt3);
else
zgradpulse(-ihh*gzlvl3, gt3);
dec2phase(t3);
delay(2.0e-4);
dec2rgpulse(calN*pwN, t3, 0.0, 0.0);
txphase(zero);
decphase(zero);
/* xxxxxxxxxxxxxxxxxx OPTIONS FOR N15 RELAXATION xxxxxxxxxxxxxxxxxxxx */
if ( (T1[A]=='y') || (T1rho[A]=='y') || (T2[A]=='y') )
{
dec2phase(one);
zgradpulse(gzlvl4, gt4); /* 2.0*GRADIENT_DELAY */
delay(tNH - gt4 - 2.0*GRADIENT_DELAY);
sim3pulse(2.0*pw, 0.0, 2.0*pwN, zero, zero, one, 0.0, 0.0);
zgradpulse(gzlvl4, gt4); /* 2.0*GRADIENT_DELAY */
delay(tNH - gt4 - 2.0*GRADIENT_DELAY);
}
/* xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx */
if (T1[A]=='y')
{
dec2rgpulse(pwN, one, 0.0, 0.0);
dec2phase(three);
zgradpulse(gzlvl0, gt0); /* 2.0*GRADIENT_DELAY */
delay(2.5e-3 - gt0 - 2.0*GRADIENT_DELAY - pw);
rgpulse(2.0*pw, zero, 0.0, 0.0);
delay(2.5e-3 - pw);
ncyc = (100.0*relaxT);
initval(ncyc,v4);
if (ncyc > 0)
{loop(v4,v5);
delay(2.5e-3 - pw);
rgpulse(2.0*pw, two, 0.0, 0.0);
delay(2.5e-3 - pw);
delay(2.5e-3 - pw);
rgpulse(2.0*pw, zero, 0.0, 0.0);
delay(2.5e-3 - pw);
endloop(v5);}
dec2rgpulse(pwN, three, 0.0, 0.0);
}
/* xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx */
/* Theory suggests 8.0 is better than 2PI as RF */
/* field multiplier and experiment confirms this.*/
if (T1rho[A]=='y') /* Shift evolution of 2.0*pwN/PI for one pulse */
{ /* at end left unrefocused as for normal sequence*/
delay(1.0/(8.0*slNrf) - pwN);
decrgpulse(pwN, zero, 0.0, 0.0);
dec2power(slNlvl);
/* minimum 5ms spinlock to dephase */
dec2rgpulse((2.5e-3-pw), zero, 0.0, 0.0); /* spins not locked */
sim3pulse(2.0*pw, 0.0, 2.0*pw, zero, zero, zero, 0.0, 0.0);
dec2rgpulse((2.5e-3-pw), zero, 0.0, 0.0);
ncyc = 100.0*relaxT;
initval(ncyc,v4); if (ncyc > 0)
{loop(v4,v5);
dec2rgpulse((2.5e-3-pw), zero, 0.0, 0.0);
sim3pulse(2.0*pw, 0.0, 2.0*pw, two, zero, zero, 0.0, 0.0);
dec2rgpulse((2.5e-3-pw), zero, 0.0, 0.0);
dec2rgpulse((2.5e-3-pw), zero, 0.0, 0.0);
sim3pulse(2.0*pw, 0.0, 2.0*pw, zero, zero, zero, 0.0, 0.0);
dec2rgpulse((2.5e-3-pw), zero, 0.0, 0.0);
endloop(v5);}
dec2power(pwNlvl);
decrgpulse(pwN, zero, 0.0, 0.0);
delay(1.0/(8.0*slNrf) + 2.0*pwN/PI - pwN);
}
/* xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx */
if (T2[A]=='y')
{
dec2phase(zero);
initval(0.0,v3); initval(180.0,v4);
if (BPpwrlimits > 0.5)
{
dec2power(pwNlvl-3.0); /* reduce for probe protection */
pwN=pwN*compN*1.4;
}
ncyc = 100.0*relaxT;
initval(ncyc,v5);
loop(v5,v6);
initval(3.0,v7);
loop(v7,v8);
delay(0.625e-3 - pwN);
dec2rgpulse(2.0*pwN, zero, 0.0, 0.0);
delay(0.625e-3 - pwN);
endloop(v8);
delay(0.625e-3 - pwN - SAPS_DELAY);
add(v4,v3,v3); obsstepsize(1.0); xmtrphase(v3); /* SAPS_DELAY */
dec2rgpulse(2.0*pwN, zero, 0.0, 0.0);
delay(0.625e-3 - pwN - pw);
rgpulse(2*pw, zero, 0.0, 0.0);
delay(0.625e-3 - pwN - pw );
dec2rgpulse(2.0*pwN, zero, 0.0, 0.0);
xmtrphase(zero); /* SAPS_DELAY */
delay(0.625e-3 - pwN - SAPS_DELAY);
initval(3.0,v9);
loop(v9,v10);
delay(0.625e-3 - pwN);
dec2rgpulse(2.0*pwN, zero, 0.0, 0.0);
delay(0.625e-3 - pwN);
endloop(v10);
endloop(v6);
if (BPpwrlimits > 0.5)
{
dec2power(pwNlvl); /* restore normal value */
pwN=getval("pwN");
}
}
/* xxxxxxxxxxxxxxxxxx OPTIONS FOR N15 EVOLUTION xxxxxxxxxxxxxxxxxxxxx */
txphase(zero);
dec2phase(t9);
if ( (NH2only[A]=='y') || (T1[A]=='y') || (T1rho[A]=='y') || (T2[A]=='y') )
{
delay(tau1);
/* optional sech/tanh pulse in middle of t1 */
if (C13refoc[A]=='y') /* WFG_START_DELAY */
{decshaped_pulse("stC200", 1.0e-3, zero, 0.0, 0.0);
delay(tNH - 1.0e-3 - WFG_START_DELAY - 2.0*pw);}
else
{delay(tNH - 2.0*pw);}
rgpulse(2.0*pw, zero, 0.0, 0.0);
if (tNH < gt1 + 1.99e-4) delay(gt1 + 1.99e-4 - tNH);
delay(tau1);
dec2rgpulse(2.0*pwN, t9, 0.0, 0.0);
if (mag_flg[A] == 'y') magradpulse(gzcal*gzlvl1, gt1);
else zgradpulse(gzlvl1, gt1); /* 2.0*GRADIENT_DELAY */
txphase(t4);
dec2phase(t10);
if (tNH > gt1 + 1.99e-4) delay(tNH - gt1 - 2.0*GRADIENT_DELAY);
else delay(1.99e-4 - 2.0*GRADIENT_DELAY);
}
else if (TROSY[A]=='y')
{
if ( (C13refoc[A]=='y') && (tau1 > 0.5e-3 + WFG2_START_DELAY) )
{delay(tau1 - 0.5e-3 - WFG2_START_DELAY); /* WFG2_START_DELAY */
decshaped_pulse("stC200", 1.0e-3, zero, 0.0, 0.0);
delay(tau1 - 0.5e-3);}
else delay(2.0*tau1);
if (mag_flg[A] == 'y') magradpulse(gzcal*gzlvl1, gt1);
else zgradpulse(gzlvl1, gt1); /* 2.0*GRADIENT_DELAY */
delay(2.0e-4 - 2.0*GRADIENT_DELAY);
dec2rgpulse(2.0*pwN, t9, 0.0, 0.0);
txphase(three);
delay(gt1 + 2.0e-4 - pwHs - 1.0e-4 - 2.0*pwr_dly);
obspower(tpwrs);
if (tpwrsf<4095.0) obspwrf(tpwrsf);
shaped_pulse("H2Osinc", pwHs, three, 5.0e-5, 0.0);
obspower(tpwr);
if (tpwrsf<4095.0) obspwrf(4095.0);
txphase(t4);
delay(5.0e-5);
}
else
{ /* fully-coupled spectrum */
if (dm2[C]=='n') {rgpulse(2.0*pw, zero, 0.0, 0.0); pw=0.0;}
if ( (C13refoc[A]=='y') && (tau1 > 0.5e-3 + WFG2_START_DELAY) )
{delay(tau1 - 0.5e-3 - WFG2_START_DELAY); /* WFG2_START_DELAY */
simshaped_pulse("", "stC200", 2.0*pw, 1.0e-3, zero, zero, 0.0, 0.0);
delay(tau1 - 0.5e-3);
delay(gt1 + 2.0e-4);}
else
{delay(tau1);
rgpulse(2.0*pw, zero, 0.0, 0.0);
delay(gt1 + 2.0e-4 - 2.0*pw);
delay(tau1);}
pw=getval("pw");
dec2rgpulse(2.0*pwN, t9, 0.0, 0.0);
if (mag_flg[A] == 'y') magradpulse(gzcal*gzlvl1, gt1);
else zgradpulse(gzlvl1, gt1); /* 2.0*GRADIENT_DELAY */
txphase(t4);
dec2phase(t10);
delay(2.0e-4 - 2.0*GRADIENT_DELAY);
}
if (T1rho[A]=='y') delay(POWER_DELAY);
/* xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx */
if (TROSY[A]=='y') rgpulse(pw, t4, 0.0, 0.0);
else sim3pulse(pw, 0.0, pwN, t4, zero, t10, 0.0, 0.0);
txphase(zero);
dec2phase(zero);
zgradpulse(gzlvl5, gt5);
if (TROSY[A]=='y') delay(lambda - 0.65*(pw + pwN) - gt5);
else delay(lambda - 1.3*pwN - gt5);
sim3pulse(2.0*pw, 0.0, 2.0*pwN, zero, zero, zero, 0.0, 0.0);
zgradpulse(gzlvl5, gt5);
txphase(one);
dec2phase(t11);
delay(lambda - 1.3*pwN - gt5);
sim3pulse(pw, 0.0, pwN, one, zero, t11, 0.0, 0.0);
txphase(zero);
dec2phase(zero);
zgradpulse(1.5*gzlvl5, gt5);
delay(lambda - 1.3*pwN - gt5);
sim3pulse(2.0*pw, 0.0, 2.0*pwN, zero, zero, zero, 0.0, 0.0);
dec2phase(t10);
zgradpulse(1.5*gzlvl5, gt5);
if (TROSY[A]=='y') delay(lambda - 1.6*pwN - gt5);
else delay(lambda - 0.65*pwN - gt5);
if (TROSY[A]=='y') dec2rgpulse(pwN, t10, 0.0, 0.0);
else rgpulse(pw, zero, 0.0, 0.0);
delay((gt1/10.0) + 1.0e-4 +gstab - 0.65*pw + 2.0*GRADIENT_DELAY + POWER_DELAY);
if ( dm3[B] == 'y' ) /* turns off 2H decoupling */
{
setstatus(DEC3ch, FALSE, 'c', FALSE, dmf3);
dec3rgpulse(1/dmf3,three,2.0e-6,2.0e-6);
dec3blank();
lk_autotrig(); /* resumes lock pulsing */
}
rgpulse(2.0*pw, zero, 0.0, 0.0);
dec2power(dpwr2); /* POWER_DELAY */
if (mag_flg[A] == 'y') magradpulse(icosel*gzcal*gzlvl2, 0.1*gt1);
else zgradpulse(icosel*gzlvl2, 0.1*gt1); /* 2.0*GRADIENT_DELAY */
if(Cdecflg[0] == 'y')
{
delay(gstab-2.0*POWER_DELAY-PRG_START_DELAY+rof2);
rcvron();
statusdelay(C,1.0e-4);
if (dm3[B] == 'y')
{
delay(1/dmf3);
lk_sample();
}
setreceiver(t12);
pbox_decon(&Cdseq);
if(Hdecflg[0] == 'y')
homodec(&HHdseq);
}
else
{
delay(gstab+rof2);
rcvron();
statusdelay(C,1.0e-4);
if (dm3[B] == 'y')
{
delay(1/dmf3);
lk_sample();
}
setreceiver(t12);
if(Hdecflg[0] == 'y')
homodec(&HHdseq);
}
}
// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
// acceleration within the allotted distance.
FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity, float distance) {
return sqrt(target_velocity*target_velocity-2*acceleration*distance);
}
int doAllFeatures()
{
/* Initial biases */
{
int u,m;
//memset(sdbU,0,NENTRIES*sizeof(float));
//memset(sdsU,0,((unsigned int)NENTRIES)*((unsigned int)NFEATURES)*sizeof(float));
ZERO(bVbin);
ZERO(alphasU);
ZERO(alphabU);
for(u=0;u<NUSERS;u++) {
bU[u]=0.0;
}
for(m=0;m<NMOVIES;m++) {
bV[m]=0.0;
}
}
/* Initial estimation for current feature */
{
int u,m,f;
double uvInit = sqrt(GLOBAL_MEAN/NFEATURES);
for(u=0;u<NUSERS;u++) {
for(f=0;f<NFEATURES;f++) {
sU[u][f] = uvInit * (rand()%14000 + 2000) * 0.000001235f;
}
}
for(m=0;m<NMOVIES;m++) {
for(f=0;f<NFEATURES;f++) {
sV[m][f]= uvInit * (rand()%14000 + 2000) * -0.000001235f;
sY[m][f]=0.0;
}
}
}
/* Optimize current feature */
double nrmse=2., last_rmse=10.;
double prmse = 0, last_prmse=0;
double thr=sqrt(1.-E);
int loopcount=0;
double Gamma1 = G1;
double Gamma2 = G2;
double Gamma4 = G4;
while( ((nrmse < (last_rmse-E) && prmse<last_prmse) || loopcount < 15) && loopcount < 40 ) {
last_rmse=nrmse;
last_prmse=prmse;
clock_t t0=clock();
loopcount++;
double aErrAvg=0;
double aEDAvg=0;
double astepSuAvg=0;
double astepSvAvg=0;
double astepSyAvg=0;
double aasU=0, aabU=0, abU=0, abV=0, asU=0, asV=0, asY=0;
long n1=0, n2=0, n3=0;
int u,m, f;
for(u=0;u<NUSERS;u++) {
// Calculate sumY and NuSY for each factor
double sumY[NFEATURES];
ZERO(sumY);
double lNuSY[NFEATURES];
ZERO(lNuSY);
int base0=useridx[u][0];
int d0=UNTRAIN(u);
int j;
int f;
int dall=UNALL(u);
double NuS = 1.0/sqrt(dall);
for(j=0;j<dall;j++) {
int mm=userent[base0+j]&USER_MOVIEMASK;
for(f=0;f<NFEATURES;f++)
sumY[f]+=sY[mm][f];
}
for(f=0;f<NFEATURES;f++) {
lNuSY[f] = NuS * sumY[f];
}
double ycontrib[NFEATURES];
ZERO(ycontrib);
// For all rated movies
for(j=0;j<d0;j++) {
int entloc = base0+j;
unsigned int sdloc = sdbin[entloc];
int m=userent[entloc]&USER_MOVIEMASK;
int day=userent[entloc]>>(USER_LMOVIEMASK+3);
double devuhat = DEVuHat[entloc];
// Figure out the current error
double ee=err[entloc];
double e2 = ee;
//e2 -= (bU[u] + bV[m] + bVbin[m][dbin(day)] + sdbU[sdloc] + alphabU[u] * devuhat);
e2 -= bU[u];
e2 -= bV[m];
e2 -= bVbin[m][dbin(day)];
e2 -= sdbU[sdloc];
e2 -= alphabU[u] * devuhat;
for (f=0; f<NFEATURES; f++)
e2 -= (( sU[u][f] + sdsU[sdloc+f*NENTRIES] + alphasU[u][f] * devuhat + lNuSY[f]) * sV[m][f]);
int r=(userent[entloc]>>USER_LMOVIEMASK)&7;
r++;
double rui = r - e2;
if ( rui > 5.00 )
e2 += (rui-5.0);
else if (rui < 1.0)
e2 -= (1.0 - rui);
// Train the biases
double bUu = bU[u];
double bVm = bV[m];
double abUu = alphabU[u];
double sdbUu= sdbU[sdloc];
bU[u] += Gamma1 * (e2 - bUu * LbU);
bV[m] += Gamma1 * (e2 - bVm * LbV);
alphabU[u] += Gamma4 * (e2 * devuhat - (LabU) * abUu);
sdbU[sdloc] += Gamma4 / TOTAL_DAYS_RANGE * (e2 - (LbU) * sdbUu);
bVbin[m][dbin(day)] += Gamma4 / 30.0 * (e2 - bVbin[m][dbin(day)] * (LbV));
aErrAvg+=fabs(e2);
aEDAvg+=fabs(e2*devuhat);
abU += fabs(bUu);
abV += fabs(bVm);
aabU += fabs(alphabU[u]);
n1++;
// update U V and slope component of Y
double yfactor = NuS;
for (f=0; f<NFEATURES; f++) {
double sUu = sU[u][f];
double sVm = sV[m][f];
double sdsUu = sdsU[sdloc+f*NENTRIES];
double asUu = alphasU[u][f];
sU[u][f] += ((Gamma2) * ((e2 * sVm) - LsU * sUu));
sV[m][f] += ((Gamma2) * ((e2 * (sUu + sdsUu + asUu * devuhat + lNuSY[f])) - LsV * sVm));
alphasU[u][f] += Gamma4 * (e2 * devuhat - (LabU) * asUu);
sdsU[sdloc+f*NENTRIES] += Gamma4 /TOTAL_DAYS_RANGE * (e2 * sVm - (LsU) * sdsUu);
asU += fabs(sUu);
asV += fabs(sVm);
aasU += fabs(asUu);
astepSuAvg+=fabs(e2 * sVm);
astepSvAvg+=fabs(e2 * (sUu + sdsUu + asUu * devuhat + lNuSY[f]));
n2++;
ycontrib[f] += e2 * sVm * yfactor;
}
}
// Train Ys over all known movies for user
for(j=0;j<dall;j++) {
int m=userent[base0+j]&USER_MOVIEMASK;
for (f=0; f<NFEATURES; f++) {
double sYm = sY[m][f];
sY[m][f] += Gamma2 * (ycontrib[f] - LsY * sYm);
asY += fabs(sYm);
astepSyAvg+=fabs(ycontrib[f]);
n3++;
}
}
}
aErrAvg/=n1;
aEDAvg/=n1;
astepSuAvg/=n2;
astepSvAvg/=n2;
astepSyAvg/=n3;
aasU/=n2,aabU/=n1, abU/=n1, abV/=n1, asU/=n2, asV/=n2, asY/=n2;
double bUREG = 1.9074 / 100.0 * aErrAvg / abU;
double bVREG = 1.9074 / 100.0 * aErrAvg / abV;
double abUREG= 1.9074 / 100.0 * aEDAvg / aabU;
double sUREG = 1.9074 / 100.0 * astepSuAvg / asU;
double sVREG = 1.9074 / 100.0 * astepSvAvg / asV;
double sYREG = 1.9074 / 100.0 * astepSyAvg / asY;
double asUREG= 1.9074 / 100.0 * aEDAvg / aasU;
printf("NREG - bU: %f bV: %f, sU: %f, sV: %f, sY: %f, abU: %f, asU: %f\n", bUREG, bVREG, sUREG, sVREG, sYREG, abUREG, asUREG);
// Report rmse for main loop
nrmse=0.;
int ntrain=0;
int elcnt=0;
int k=2;
int n=0;
double s=0.;
for(u=0;u<NUSERS;u++) {
int base0=useridx[u][0];
int d0=UNTRAIN(u);
int j;
// Setup the Ys again
double sumY[NFEATURES];
ZERO(sumY);
double lNuSY[NFEATURES];
ZERO(lNuSY);
int dall=UNALL(u);
double NuS = 1.0/sqrt(dall);
for(j=0;j<dall;j++) {
int mm=userent[base0+j]&USER_MOVIEMASK;
for(f=0;f<NFEATURES;f++)
sumY[f]+=sY[mm][f];
}
for(f=0;f<NFEATURES;f++)
lNuSY[f] = NuS * sumY[f];
// For all rated movies
for(j=0;j<d0;j++) {
int entloc = base0+j;
unsigned int sdloc = sdbin[entloc];
int m=userent[entloc]&USER_MOVIEMASK;
int day=userent[entloc]>>(USER_LMOVIEMASK+3);
double devuhat = DEVuHat[entloc];
// Figure out the current error
double ee=err[entloc];
double e2 = ee;
e2 -= (bU[u] + bV[m] + bVbin[m][dbin(day)] + sdbU[sdloc] + alphabU[u] * devuhat);
for (f=0; f<NFEATURES; f++)
e2 -= (( sU[u][f] + sdsU[sdloc+f*NENTRIES] + alphasU[u][f] * devuhat + lNuSY[f]) * sV[m][f]);
//for(j=0;j<d0;j++) {
//int m=userent[base0+j]&USER_MOVIEMASK;
//double ee = err[base0+j];
//double e2 = ee;
//e2 -= (bU[u] + bV[m]);
//for (f=0; f<NFEATURES; f++)
//e2 -= ( (sU[u][f] + lNuSY[f]) * sV[m][f]);
int r=(userent[base0+j]>>USER_LMOVIEMASK)&7;
r++;
double rui = r - e2;
if ( rui > 5.00 )
e2 += (rui-5.0);
else if (rui < 1.0)
e2 -= (1.0 - rui);
if( elcnt++ == 5000 ) {
printf("0 E: %f \t NE: %f\tNuSY: %f\tsV: %f\tsU: %f\tbU: %f\tbV: %f\tsY: %f\tU: %d\tM: %d\n", ee, e2, lNuSY[0], sV[m][0], sU[u][0], bU[u], bV[m], sY[m][0],u, m);
printf("1 E: %f \t NE: %f\tNuSY: %f\tsV: %f\tsU: %f\tbU: %f\tbV: %f\tsY: %f\tU: %d\tM: %d\n", ee, e2, lNuSY[1], sV[m][1], sU[u][1], bU[u], bV[m], sY[m][1],u, m);
printf("2 E: %f \t NE: %f\tNuSY: %f\tsV: %f\tsU: %f\tbU: %f\tbV: %f\tsY: %f\tU: %d\tM: %d\n", ee, e2, lNuSY[2], sV[m][2], sU[u][2], bU[u], bV[m], sY[m][2],u, m);
printf("3 E: %f \t NE: %f\tNuSY: %f\tsV: %f\tsU: %f\tbU: %f\tbV: %f\tsY: %f\tU: %d\tM: %d\n", ee, e2, lNuSY[3], sV[m][3], sU[u][3], bU[u], bV[m], sY[m][3],u, m);
fflush(stdout);
}
nrmse+=e2*e2;
}
ntrain+=d0;
// Sum up probe rmse
int i;
int base=useridx[u][0];
for(i=1;i<k;i++) base+=useridx[u][i];
int d=useridx[u][k];
for(i=0; i<d;i++) {
int entloc = base+i;
unsigned int sdloc = sdbin[entloc];
int m=userent[entloc]&USER_MOVIEMASK;
int day=userent[entloc]>>(USER_LMOVIEMASK+3);
double devuhat = DEVuHat[entloc];
//double e=err[entloc];
//e-=(bU[u] + bV[m]);
//for (f=0; f<NFEATURES; f++)
//e-=((sU[u][f] + lNuSY[f]) * sV[m][f]);
double ee=err[entloc];
double e = ee;
e -= (bU[u] + bV[m] + bVbin[m][dbin(day)] + sdbU[sdloc] + alphabU[u] * devuhat);
for (f=0; f<NFEATURES; f++)
e -= (( sU[u][f] + sdsU[sdloc+f*NENTRIES] + alphasU[u][f] * devuhat + lNuSY[f]) * sV[m][f]);
int r=(userent[entloc]>>USER_LMOVIEMASK)&7;
r++;
double rui = r - e;
if ( rui > 5.00 )
e += (rui-5.0);
else if (rui < 1.0)
e -= (1.0 - rui);
s+=e*e;
}
n+=d;
}
nrmse=sqrt(nrmse/ntrain);
prmse = sqrt(s/n);
lg("%f\t%f\t%f\n",nrmse,prmse,(clock()-t0)/(double)CLOCKS_PER_SEC);
Gamma1 *= 0.90;
Gamma2 *= 0.90;
Gamma4 *= 0.90;
}
/* Perform a final iteration in which the errors are clipped and stored */
removeUV();
//if(save_model) {
//dappend_bin(fnameV,sV,NMOVIES);
//dappend_bin(fnameU,sU,NUSERS);
//}
return 1;
}
int main(int argc, char** argv)
{
try
{
Sundance::init(&argc, &argv);
/* We will do our linear algebra using Epetra */
VectorType<double> vecType = new EpetraVectorType();
/* Create a mesh. It will be of type BasisSimplicialMesh, and will
* be built using a PartitionedLineMesher. */
MeshType meshType = new BasicSimplicialMeshType();
MeshSource mesher = new PartitionedLineMesher(0.0, 1.0, 10, meshType);
Mesh mesh = mesher.getMesh();
/* Create a cell filter that will identify the maximal cells
* in the interior of the domain */
CellFilter interior = new MaximalCellFilter();
/* Create unknown and test functions, discretized using first-order
* Lagrange interpolants */
Expr u = new UnknownFunction(new Lagrange(1), "u");
Expr v = new TestFunction(new Lagrange(1), "v");
/* We need a quadrature rule for doing the integrations */
QuadratureFamily quad = new GaussianQuadrature(2);
/* Define the weak form */
Expr eqn = Integral(interior, v*(u-1.0), quad);
Expr bc;
/* We can now set up the linear problem! */
std::cerr << "setting up linear problem" << std::endl;
LinearProblem prob(mesh, eqn, bc, v, u, vecType);
#ifdef HAVE_CONFIG_H
ParameterXMLFileReader reader(searchForFile("SolverParameters/bicgstab.xml"));
#else
ParameterXMLFileReader reader("bicgstab.xml");
#endif
ParameterList solverParams = reader.getParameters();
std::cerr << "params = " << solverParams << std::endl;
LinearSolver<double> solver
= LinearSolverBuilder::createSolver(solverParams);
std::cerr << "solving problem" << std::endl;
Expr soln = prob.solve(solver);
Expr exactSoln = 1.0;
Expr errExpr = Integral(interior,
pow(soln-exactSoln, 2),
new GaussianQuadrature(4));
std::cerr << "setting up norm" << std::endl;
double errorSq = evaluateIntegral(mesh, errExpr);
std::cerr << "error norm = " << sqrt(errorSq) << std::endl << std::endl;
double tol = 1.0e-12;
Sundance::passFailTest(sqrt(errorSq), tol);
}
catch(std::exception& e)
{
std::cerr << e.what() << std::endl;
}
Sundance::finalize(); return Sundance::testStatus();
}
static int Psor_DownOut(double s,NumFunc_1 *p,double l,double rebate,double t,double r,double divid,double sigma,int N,int M,double theta,double omega,double epsilon,double *ptprice,double *ptdelta)
{
int Index,PriceIndex,TimeIndex;
int j,loops;
double k,vv,loc,h,z,alpha,beta,gamma,y,alpha1,beta1,gamma1,down,upwind_alphacoef;
double error,norm,x,pricenh,pricen2h,priceph;
double *P,*Obst,*Rhs;
/*Memory Allocation*/
P= malloc((N+2)*sizeof(double));
if (P==NULL)
return MEMORY_ALLOCATION_FAILURE;
Obst= malloc((N+2)*sizeof(double));
if (Obst==NULL)
return MEMORY_ALLOCATION_FAILURE;
Rhs= malloc((N+2)*sizeof(double));
if (Rhs==NULL)
return MEMORY_ALLOCATION_FAILURE;
/*Time Step*/
k=t/(double)M;
/*Space Localisation*/
vv=0.5*SQR(sigma);
z=(r-divid)-vv;
loc=sigma*sqrt(t)*sqrt(log(1.0/PRECISION))+fabs(z)*t;
/*Space Step*/
x=log(s);
down=log(l);
h=(x+loc-down)/(double)(N+1);
/*Coefficient of diffusion augmented*/
if ((h*fabs(z))<=vv)
upwind_alphacoef=0.5;
else {
if (z>0.) upwind_alphacoef=0.0;
else upwind_alphacoef=1.0;
}
vv-=z*h*(upwind_alphacoef-0.5);
/*Lhs factor of theta-schema*/
alpha=theta*k*(-vv/(h*h)+z/(2.0*h));
beta=1.0+k*theta*(r+2.*vv/(h*h));
gamma=k*theta*(-vv/(h*h)-z/(2.0*h));
/*Rhs factor of theta-schema*/
alpha1=k*(1.0-theta)*(vv/(h*h)-z/(2.0*h));
beta1=1.0-k*(1.0-theta)*(r+2.*vv/(h*h));
gamma1=k*(1.0-theta)*(vv/(h*h)+z/(2.0*h));
/*Terminal Values*/
for(PriceIndex=1;PriceIndex<=N+1;PriceIndex++) {
Obst[PriceIndex]=(p->Compute)(p->Par,exp(down+(double)PriceIndex*h));
P[PriceIndex]= Obst[PriceIndex];
}
P[0]=rebate;
/*Finite Difference Cycle*/
for(TimeIndex=1;TimeIndex<=M;TimeIndex++)
{
/*Init Rhs*/
for(j=1;j<=N;j++)
Rhs[j]=P[j]*beta1+alpha1*P[j-1]+gamma1*P[j+1];
/*Psor Cycle*/
loops=0;
do
{
error=0.;
norm=0.;
for(j=1;j<=N;j++)
{
y=(Rhs[j]-alpha*P[j-1]-gamma*P[j+1])/beta;
y=MAX(Obst[j],P[j]+omega*(y-P[j]));
error+=(double)(j+1)*fabs(y-P[j]);
norm+=fabs(y);
P[j]=y;
}
if (norm<1.0) norm=1.0;
error=error/norm;
loops++;
}
while ((error>epsilon) && (loops<MAXLOOPS));
}
Index=(int)floor((x-down)/h);
/*Price*/
*ptprice=P[Index]+(P[Index+1]-P[Index])*(exp(x)-exp(down+Index*h))/(exp(down+(Index+1)*h)-exp(down+Index*h));
/*Delta*/
pricenh=P[Index+1]+(P[Index+2]-P[Index+1])*(exp(x+h)-exp(down+(Index+1)*h))/(exp(down+(Index+2)*h)-exp(down+(Index+1)*h));
if (Index>0) {
priceph=P[Index-1]+(P[Index]-P[Index-1])*(exp(x-h)-exp(down+(Index-1)*h))/(exp(down+(Index)*h)-exp(down+(Index-1)*h));
*ptdelta=(pricenh-priceph)/(2*s*h);
} else {
pricen2h=P[Index+2]+(P[Index+3]-P[Index+2])*(exp(x+2*h)-exp(down+(Index+2)*h))/(exp(down+(Index+3)*h)-exp(down+(Index+2)*h));
*ptdelta=(4*pricenh-pricen2h-3*(*ptprice))/(2.*s*h);
}
/*Memory Desallocation*/
free(P);
free(Obst);
free(Rhs);
return OK;
}
void Yadro::getStartVerTexArrays(GLfloat sferaV[80][3][3], int start)
{
const GLfloat pi=3.141593, k=pi/180;
GLfloat x21 = sferaV[start][0][0]; GLfloat x22 = sferaV[start][1][0]; GLfloat x23 = sferaV[start][2][0];
GLfloat y21 = sferaV[start][0][1]; GLfloat y22 = sferaV[start][1][1]; GLfloat y23 = sferaV[start][2][1];
GLfloat z21 = sferaV[start][0][2]; GLfloat z22 = sferaV[start][1][2]; GLfloat z23 = sferaV[start][2][2];
GLfloat x2 = (x21+x22+x23)/3;
GLfloat y2 = (y21+y22+y23)/3;
GLfloat z2 = (z21+z22+z23)/3;
GLfloat r2 = sqrt(x2*x2 + y2*y2 + z2*z2);
GLfloat theta2 = acos(z2/r2);
GLfloat phi2 = atan2(y2,x2);
for (int i = 0; i < 6; i++) {
GLfloat x1 = VertexArrayYadro[i][0];
GLfloat y1 = VertexArrayYadro[i][1];
GLfloat z1 = VertexArrayYadro[i][2];
GLfloat r1 = sqrt(x1*x1 + y1*y1 + z1*z1);
GLfloat theta1 = acos(z1/r1);
GLfloat phi1 = atan2(y1,x1);
if (delta2 == 0.0f) {
// Установка начальных параметров запуска
delta2 = 1.0f;
shag = 2;
deltaR = 0.15f;
deltaTh = fabs(theta2 - theta1);
deltaPh = fabs( phi2 - phi1 );
deltaR = fabs( r2 - r1 );
deltaTh = deltaTh / (GLfloat)shag;
deltaPh = deltaPh / (GLfloat)shag;
if ( theta1 < theta2) {
signTh = 1;
} else {
signTh = 0;
}
if ( phi1 < phi2 ) {
signPh = 1;
} else {
signPh = 0;
}
}
if ( signTh == 1 ) {
theta1+=deltaTh;
} else {
theta1-=deltaTh;
}
if ( signPh == 1 ) {
phi1+=deltaPh;
} else {
phi1-=deltaPh;
}
r1+= deltaR;
if (shag > 0) {
VertexArrayYadro[i][0]=r1*sin(theta1)*cos(phi1);
VertexArrayYadro[i][1]=r1*sin(theta1)*sin(phi1);
VertexArrayYadro[i][2]=r1*cos(theta1);
int h = 43;
if ( i == 5 ) {
shag--;
}
}
if (shag == 0 && pli < 80) {
//delta2 = 0.0f;
//sfera.kill(pli+plifinish);
//pli+=plifinish;
}
}
}
/* Subroutine */ int cstegr_(char *jobz, char *range, integer *n, real *d__,
real *e, real *vl, real *vu, integer *il, integer *iu, real *abstol,
integer *m, real *w, complex *z__, integer *ldz, integer *isuppz,
real *work, integer *lwork, integer *iwork, integer *liwork, integer *
info, ftnlen jobz_len, ftnlen range_len)
{
/* System generated locals */
integer z_dim1, z_offset, i__1, i__2;
real r__1, r__2;
/* Builtin functions */
double sqrt(doublereal);
/* Local variables */
static integer i__, j, jj;
static real eps, tol, tmp;
static integer iend;
static real rmin, rmax;
static integer itmp;
static real tnrm, scale;
extern logical lsame_(char *, char *, ftnlen, ftnlen);
static integer iinfo;
extern /* Subroutine */ int sscal_(integer *, real *, real *, integer *),
cswap_(integer *, complex *, integer *, complex *, integer *);
static integer lwmin;
static logical wantz, alleig;
static integer ibegin;
static logical indeig;
static integer iindbl;
static logical valeig;
extern doublereal slamch_(char *, ftnlen);
extern /* Subroutine */ int claset_(char *, integer *, integer *, complex
*, complex *, complex *, integer *, ftnlen);
static real safmin;
extern /* Subroutine */ int xerbla_(char *, integer *, ftnlen);
static real bignum;
static integer iindwk, indgrs, indwof;
extern /* Subroutine */ int clarrv_(integer *, real *, real *, integer *,
integer *, real *, integer *, real *, real *, complex *, integer *
, integer *, real *, integer *, integer *), slarre_(integer *,
real *, real *, real *, integer *, integer *, integer *, real *,
real *, real *, real *, integer *);
static real thresh;
static integer iinspl, indwrk, liwmin;
extern doublereal slanst_(char *, integer *, real *, real *, ftnlen);
static integer nsplit;
static real smlnum;
static logical lquery;
/* -- LAPACK computational routine (version 3.0) -- */
/* Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd., */
/* Courant Institute, Argonne National Lab, and Rice University */
/* October 31, 1999 */
/* .. Scalar Arguments .. */
/* .. */
/* .. Array Arguments .. */
/* .. */
/* Purpose */
/* ======= */
/* CSTEGR computes selected eigenvalues and, optionally, eigenvectors */
/* of a real symmetric tridiagonal matrix T. Eigenvalues and */
/* eigenvectors can be selected by specifying either a range of values */
/* or a range of indices for the desired eigenvalues. The eigenvalues */
/* are computed by the dqds algorithm, while orthogonal eigenvectors are */
/* computed from various ``good'' L D L^T representations (also known as */
/* Relatively Robust Representations). Gram-Schmidt orthogonalization is */
/* avoided as far as possible. More specifically, the various steps of */
/* the algorithm are as follows. For the i-th unreduced block of T, */
/* (a) Compute T - sigma_i = L_i D_i L_i^T, such that L_i D_i L_i^T */
/* is a relatively robust representation, */
/* (b) Compute the eigenvalues, lambda_j, of L_i D_i L_i^T to high */
/* relative accuracy by the dqds algorithm, */
/* (c) If there is a cluster of close eigenvalues, "choose" sigma_i */
/* close to the cluster, and go to step (a), */
/* (d) Given the approximate eigenvalue lambda_j of L_i D_i L_i^T, */
/* compute the corresponding eigenvector by forming a */
/* rank-revealing twisted factorization. */
/* The desired accuracy of the output can be specified by the input */
/* parameter ABSTOL. */
/* For more details, see "A new O(n^2) algorithm for the symmetric */
/* tridiagonal eigenvalue/eigenvector problem", by Inderjit Dhillon, */
/* Computer Science Division Technical Report No. UCB/CSD-97-971, */
/* UC Berkeley, May 1997. */
/* Note 1 : Currently CSTEGR is only set up to find ALL the n */
/* eigenvalues and eigenvectors of T in O(n^2) time */
/* Note 2 : Currently the routine CSTEIN is called when an appropriate */
/* sigma_i cannot be chosen in step (c) above. CSTEIN invokes modified */
/* Gram-Schmidt when eigenvalues are close. */
/* Note 3 : CSTEGR works only on machines which follow ieee-754 */
/* floating-point standard in their handling of infinities and NaNs. */
/* Normal execution of CSTEGR may create NaNs and infinities and hence */
/* may abort due to a floating point exception in environments which */
/* do not conform to the ieee standard. */
/* Arguments */
/* ========= */
/* JOBZ (input) CHARACTER*1 */
/* = 'N': Compute eigenvalues only; */
/* = 'V': Compute eigenvalues and eigenvectors. */
/* RANGE (input) CHARACTER*1 */
/* = 'A': all eigenvalues will be found. */
/* = 'V': all eigenvalues in the half-open interval (VL,VU] */
/* will be found. */
/* = 'I': the IL-th through IU-th eigenvalues will be found. */
/* ********* Only RANGE = 'A' is currently supported ********************* */
/* N (input) INTEGER */
/* The order of the matrix. N >= 0. */
/* D (input/output) REAL array, dimension (N) */
/* On entry, the n diagonal elements of the tridiagonal matrix */
/* T. On exit, D is overwritten. */
/* E (input/output) REAL array, dimension (N) */
/* On entry, the (n-1) subdiagonal elements of the tridiagonal */
/* matrix T in elements 1 to N-1 of E; E(N) need not be set. */
/* On exit, E is overwritten. */
/* VL (input) REAL */
/* VU (input) REAL */
/* If RANGE='V', the lower and upper bounds of the interval to */
/* be searched for eigenvalues. VL < VU. */
/* Not referenced if RANGE = 'A' or 'I'. */
/* IL (input) INTEGER */
/* IU (input) INTEGER */
/* If RANGE='I', the indices (in ascending order) of the */
/* smallest and largest eigenvalues to be returned. */
/* 1 <= IL <= IU <= N, if N > 0; IL = 1 and IU = 0 if N = 0. */
/* Not referenced if RANGE = 'A' or 'V'. */
/* ABSTOL (input) REAL */
/* The absolute error tolerance for the */
/* eigenvalues/eigenvectors. IF JOBZ = 'V', the eigenvalues and */
/* eigenvectors output have residual norms bounded by ABSTOL, */
/* and the dot products between different eigenvectors are */
/* bounded by ABSTOL. If ABSTOL is less than N*EPS*|T|, then */
/* N*EPS*|T| will be used in its place, where EPS is the */
/* machine precision and |T| is the 1-norm of the tridiagonal */
/* matrix. The eigenvalues are computed to an accuracy of */
/* EPS*|T| irrespective of ABSTOL. If high relative accuracy */
/* is important, set ABSTOL to DLAMCH( 'Safe minimum' ). */
/* See Barlow and Demmel "Computing Accurate Eigensystems of */
/* Scaled Diagonally Dominant Matrices", LAPACK Working Note #7 */
/* for a discussion of which matrices define their eigenvalues */
/* to high relative accuracy. */
/* M (output) INTEGER */
/* The total number of eigenvalues found. 0 <= M <= N. */
/* If RANGE = 'A', M = N, and if RANGE = 'I', M = IU-IL+1. */
/* W (output) REAL array, dimension (N) */
/* The first M elements contain the selected eigenvalues in */
/* ascending order. */
/* Z (output) COMPLEX array, dimension (LDZ, max(1,M) ) */
/* If JOBZ = 'V', then if INFO = 0, the first M columns of Z */
/* contain the orthonormal eigenvectors of the matrix T */
/* corresponding to the selected eigenvalues, with the i-th */
/* column of Z holding the eigenvector associated with W(i). */
/* If JOBZ = 'N', then Z is not referenced. */
/* Note: the user must ensure that at least max(1,M) columns are */
/* supplied in the array Z; if RANGE = 'V', the exact value of M */
/* is not known in advance and an upper bound must be used. */
/* LDZ (input) INTEGER */
/* The leading dimension of the array Z. LDZ >= 1, and if */
/* JOBZ = 'V', LDZ >= max(1,N). */
/* ISUPPZ (output) INTEGER ARRAY, dimension ( 2*max(1,M) ) */
/* The support of the eigenvectors in Z, i.e., the indices */
/* indicating the nonzero elements in Z. The i-th eigenvector */
/* is nonzero only in elements ISUPPZ( 2*i-1 ) through */
/* ISUPPZ( 2*i ). */
/* WORK (workspace/output) REAL array, dimension (LWORK) */
/* On exit, if INFO = 0, WORK(1) returns the optimal */
/* (and minimal) LWORK. */
/* LWORK (input) INTEGER */
/* The dimension of the array WORK. LWORK >= max(1,18*N) */
/* If LWORK = -1, then a workspace query is assumed; the routine */
/* only calculates the optimal size of the WORK array, returns */
/* this value as the first entry of the WORK array, and no error */
/* message related to LWORK is issued by XERBLA. */
/* IWORK (workspace/output) INTEGER array, dimension (LIWORK) */
/* On exit, if INFO = 0, IWORK(1) returns the optimal LIWORK. */
/* LIWORK (input) INTEGER */
/* The dimension of the array IWORK. LIWORK >= max(1,10*N) */
/* If LIWORK = -1, then a workspace query is assumed; the */
/* routine only calculates the optimal size of the IWORK array, */
/* returns this value as the first entry of the IWORK array, and */
/* no error message related to LIWORK is issued by XERBLA. */
/* INFO (output) INTEGER */
/* = 0: successful exit */
/* < 0: if INFO = -i, the i-th argument had an illegal value */
/* > 0: if INFO = 1, internal error in SLARRE, */
/* if INFO = 2, internal error in CLARRV. */
/* Further Details */
/* =============== */
/* Based on contributions by */
/* Inderjit Dhillon, IBM Almaden, USA */
/* Osni Marques, LBNL/NERSC, USA */
/* Ken Stanley, Computer Science Division, University of */
/* California at Berkeley, USA */
/* ===================================================================== */
/* .. Parameters .. */
/* .. */
/* .. Local Scalars .. */
/* .. */
/* .. External Functions .. */
/* .. */
/* .. External Subroutines .. */
/* .. */
/* .. Intrinsic Functions .. */
/* .. */
/* .. Executable Statements .. */
/* Test the input parameters. */
/* Parameter adjustments */
--d__;
--e;
--w;
z_dim1 = *ldz;
z_offset = 1 + z_dim1;
z__ -= z_offset;
--isuppz;
--work;
--iwork;
/* Function Body */
wantz = lsame_(jobz, "V", (ftnlen)1, (ftnlen)1);
alleig = lsame_(range, "A", (ftnlen)1, (ftnlen)1);
valeig = lsame_(range, "V", (ftnlen)1, (ftnlen)1);
indeig = lsame_(range, "I", (ftnlen)1, (ftnlen)1);
lquery = *lwork == -1 || *liwork == -1;
lwmin = *n * 18;
liwmin = *n * 10;
*info = 0;
if (! (wantz || lsame_(jobz, "N", (ftnlen)1, (ftnlen)1))) {
*info = -1;
} else if (! (alleig || valeig || indeig)) {
*info = -2;
/* The following two lines need to be removed once the */
/* RANGE = 'V' and RANGE = 'I' options are provided. */
} else if (valeig || indeig) {
*info = -2;
} else if (*n < 0) {
*info = -3;
} else if (valeig && *n > 0 && *vu <= *vl) {
*info = -7;
} else if (indeig && *il < 1) {
*info = -8;
/* The following change should be made in DSTEVX also, otherwise */
/* IL can be specified as N+1 and IU as N. */
/* ELSE IF( INDEIG .AND. ( IU.LT.MIN( N, IL ) .OR. IU.GT.N ) ) THEN */
} else if (indeig && (*iu < *il || *iu > *n)) {
*info = -9;
} else if (*ldz < 1 || wantz && *ldz < *n) {
*info = -14;
} else if (*lwork < lwmin && ! lquery) {
*info = -17;
} else if (*liwork < liwmin && ! lquery) {
*info = -19;
}
if (*info == 0) {
work[1] = (real) lwmin;
iwork[1] = liwmin;
}
if (*info != 0) {
i__1 = -(*info);
xerbla_("CSTEGR", &i__1, (ftnlen)6);
return 0;
} else if (lquery) {
return 0;
}
/* Quick return if possible */
*m = 0;
if (*n == 0) {
return 0;
}
if (*n == 1) {
if (alleig || indeig) {
*m = 1;
w[1] = d__[1];
} else {
if (*vl < d__[1] && *vu >= d__[1]) {
*m = 1;
w[1] = d__[1];
}
}
if (wantz) {
i__1 = z_dim1 + 1;
z__[i__1].r = 1.f, z__[i__1].i = 0.f;
}
return 0;
}
/* Get machine constants. */
safmin = slamch_("Safe minimum", (ftnlen)12);
eps = slamch_("Precision", (ftnlen)9);
smlnum = safmin / eps;
bignum = 1.f / smlnum;
rmin = sqrt(smlnum);
/* Computing MIN */
r__1 = sqrt(bignum), r__2 = 1.f / sqrt(sqrt(safmin));
rmax = dmin(r__1,r__2);
/* Scale matrix to allowable range, if necessary. */
scale = 1.f;
tnrm = slanst_("M", n, &d__[1], &e[1], (ftnlen)1);
if (tnrm > 0.f && tnrm < rmin) {
scale = rmin / tnrm;
} else if (tnrm > rmax) {
scale = rmax / tnrm;
}
if (scale != 1.f) {
sscal_(n, &scale, &d__[1], &c__1);
i__1 = *n - 1;
sscal_(&i__1, &scale, &e[1], &c__1);
tnrm *= scale;
}
indgrs = 1;
indwof = (*n << 1) + 1;
indwrk = *n * 3 + 1;
iinspl = 1;
iindbl = *n + 1;
iindwk = (*n << 1) + 1;
claset_("Full", n, n, &c_b1, &c_b1, &z__[z_offset], ldz, (ftnlen)4);
/* Compute the desired eigenvalues of the tridiagonal after splitting */
/* into smaller subblocks if the corresponding of-diagonal elements */
/* are small */
thresh = eps * tnrm;
slarre_(n, &d__[1], &e[1], &thresh, &nsplit, &iwork[iinspl], m, &w[1], &
work[indwof], &work[indgrs], &work[indwrk], &iinfo);
if (iinfo != 0) {
*info = 1;
return 0;
}
if (wantz) {
/* Compute the desired eigenvectors corresponding to the computed */
/* eigenvalues */
/* Computing MAX */
r__1 = *abstol, r__2 = (real) (*n) * thresh;
tol = dmax(r__1,r__2);
ibegin = 1;
i__1 = nsplit;
for (i__ = 1; i__ <= i__1; ++i__) {
iend = iwork[iinspl + i__ - 1];
i__2 = iend;
for (j = ibegin; j <= i__2; ++j) {
iwork[iindbl + j - 1] = i__;
/* L10: */
}
ibegin = iend + 1;
/* L20: */
}
clarrv_(n, &d__[1], &e[1], &iwork[iinspl], m, &w[1], &iwork[iindbl], &
work[indgrs], &tol, &z__[z_offset], ldz, &isuppz[1], &work[
indwrk], &iwork[iindwk], &iinfo);
if (iinfo != 0) {
*info = 2;
return 0;
}
}
ibegin = 1;
i__1 = nsplit;
for (i__ = 1; i__ <= i__1; ++i__) {
iend = iwork[iinspl + i__ - 1];
i__2 = iend;
for (j = ibegin; j <= i__2; ++j) {
w[j] += work[indwof + i__ - 1];
/* L30: */
}
ibegin = iend + 1;
/* L40: */
}
/* If matrix was scaled, then rescale eigenvalues appropriately. */
if (scale != 1.f) {
r__1 = 1.f / scale;
sscal_(m, &r__1, &w[1], &c__1);
}
/* If eigenvalues are not in order, then sort them, along with */
/* eigenvectors. */
if (nsplit > 1) {
i__1 = *m - 1;
for (j = 1; j <= i__1; ++j) {
i__ = 0;
tmp = w[j];
i__2 = *m;
for (jj = j + 1; jj <= i__2; ++jj) {
if (w[jj] < tmp) {
i__ = jj;
tmp = w[jj];
}
/* L50: */
}
if (i__ != 0) {
w[i__] = w[j];
w[j] = tmp;
if (wantz) {
cswap_(n, &z__[i__ * z_dim1 + 1], &c__1, &z__[j * z_dim1
+ 1], &c__1);
itmp = isuppz[(i__ << 1) - 1];
isuppz[(i__ << 1) - 1] = isuppz[(j << 1) - 1];
isuppz[(j << 1) - 1] = itmp;
itmp = isuppz[i__ * 2];
isuppz[i__ * 2] = isuppz[j * 2];
isuppz[j * 2] = itmp;
}
}
/* L60: */
}
}
work[1] = (real) lwmin;
iwork[1] = liwmin;
return 0;
/* End of CSTEGR */
} /* cstegr_ */
void Yadro::fly(GLfloat sferaV[80][3][3]/*, int flag*/, Sfera &sfera)
{
const GLfloat pi=3.141593, k=pi/180;
GLfloat x21 = sferaV[pli+plifinish][0][0]; GLfloat x22 = sferaV[pli+plifinish][1][0]; GLfloat x23 = sferaV[pli+plifinish][2][0];
GLfloat y21 = sferaV[pli+plifinish][0][1]; GLfloat y22 = sferaV[pli+plifinish][1][1]; GLfloat y23 = sferaV[pli+plifinish][2][1];
GLfloat z21 = sferaV[pli+plifinish][0][2]; GLfloat z22 = sferaV[pli+plifinish][1][2]; GLfloat z23 = sferaV[pli+plifinish][2][2];
GLfloat x2 = (x21+x22+x23)/3;
GLfloat y2 = (y21+y22+y23)/3;
GLfloat z2 = (z21+z22+z23)/3;
GLfloat r2 = sqrt(x2*x2 + y2*y2 + z2*z2);
GLfloat theta2 = acos(z2/r2);
GLfloat phi2 = atan2(y2,x2);
for (int i = 0; i < 6; i++) {
// Расчеты координат движущегося ядра
GLfloat x1 = VertexArrayYadro[i][0];
GLfloat y1 = VertexArrayYadro[i][1];
GLfloat z1 = VertexArrayYadro[i][2];
GLfloat r1 = sqrt(x1*x1 + y1*y1 + z1*z1);
GLfloat theta1 = acos(z1/r1);
GLfloat phi1 = atan2(y1,x1);
if (delta1 == 0.0f) {
// установка начальных параметров запуска
delta1 = 1.0f;
shag = 50;
deltaR = 0.05f;
deltaTh = fabs(theta2 - theta1);
deltaPh = fabs( phi2 - phi1 );
deltaTh = deltaTh / (GLfloat)shag;
deltaPh = deltaPh / (GLfloat)shag;
if ( theta1 < theta2) {
signTh = 1;
} else {
signTh = 0;
}
if ( phi1 < phi2 ) {
signPh = 1;
} else {
signPh = 0;
}
}
// По флагам усанавливаем стороны вращения
if ( signTh == 1 ) {
theta1+=deltaTh;
} else {
theta1-=deltaTh;
}
if ( signPh == 1 ) {
phi1+=deltaPh;
} else {
phi1-=deltaPh;
}
if ( shag > 25) {
r1+= deltaR;
} else {
r1-= deltaR;
}
if (shag > 0) {
VertexArrayYadro[i][0]=r1*sin(theta1)*cos(phi1);
VertexArrayYadro[i][1]=r1*sin(theta1)*sin(phi1);
VertexArrayYadro[i][2]=r1*cos(theta1);
int h = 43;
if ( i == 5 ) {
shag--;
}
doletelo = 0;
}
if (shag == 0 && pli < 80) {
// Признак сработал. Ракета прилетела
sfera.fail(pli+plifinish);
doletelo = 1;
}
}
}
static fixed_t Length (fixed_t dx, fixed_t dy)
{
return (fixed_t)sqrt ((double)dx*(double)dx+(double)dy*(double)dy);
}
int computeBodyPoseAndMotionInfo(double **data, double **pos_data, int variableNum) {
//double data[JOINT_NUM][JOINT_DATA_NUM], double pos_data[POS_JOINT_NUM][POS_JOINT_DATA_NUM], int variableNum) {
for (int i=0; i<compareFrameNum; i++) {
for (int j=0; j<JOINT_NUM; j++) {
int frameCompareTo = findFrame(compareFrame[i]);
if (currentFramePointer == frameCompareTo) {
if (j != TORSO_JOINT_NUM) {
// rot mat
double m[3][3];
for (int k =0;k<3;k++) {
m[0][k]= prevFrame[currentFramePointer][TORSO_JOINT_NUM][0]*prevFrame[currentFramePointer][j][0+k]
+ prevFrame[currentFramePointer][TORSO_JOINT_NUM][3]*prevFrame[currentFramePointer][j][3+k]
+ prevFrame[currentFramePointer][TORSO_JOINT_NUM][6]*prevFrame[currentFramePointer][j][6+k];
m[1][k]= prevFrame[currentFramePointer][TORSO_JOINT_NUM][1]*prevFrame[currentFramePointer][j][0+k]
+ prevFrame[currentFramePointer][TORSO_JOINT_NUM][4]*prevFrame[currentFramePointer][j][3+k]
+ prevFrame[currentFramePointer][TORSO_JOINT_NUM][7]*prevFrame[currentFramePointer][j][6+k];
m[2][k]= prevFrame[currentFramePointer][TORSO_JOINT_NUM][2]*prevFrame[currentFramePointer][j][0+k]
+ prevFrame[currentFramePointer][TORSO_JOINT_NUM][5]*prevFrame[currentFramePointer][j][3+k]
+ prevFrame[currentFramePointer][TORSO_JOINT_NUM][8]*prevFrame[currentFramePointer][j][6+k];
/*printf("%.10f %.10f %.10f %.10f\n", prevFrame[currentFramePointer][TORSO_JOINT_NUM][0]*prevFrame[currentFramePointer][j][0+k]
, prevFrame[currentFramePointer][TORSO_JOINT_NUM][3]*prevFrame[currentFramePointer][j][3+k]
, prevFrame[currentFramePointer][TORSO_JOINT_NUM][6]*prevFrame[currentFramePointer][j][6+k], m[0][k]);*/
}
/*for (int ii = 0;ii<3;ii++)
for(int jj=0;jj<3;jj++)
printf("%d %d %d %.10f\n",frameCompareTo, ii,jj,m[ii][jj]); */
// quat
double q1 = 1.0/2.0 * sqrt(1 + m[0][0] - m[1][1] - m[2][2]);
double q2 = 1.0/(4.0*q1) * (m[0][1] + m[1][0]);
double q3 = 1.0/(4.0*q1) * (m[0][2] + m[2][0]);
double q4 = 1.0/(4.0*q1) * (m[2][1] - m[1][2]);
if (!isnan(q1) && !isnan(q2) && !isnan(q3) && !isnan(q4)
&& !isinf(q1) && !isinf(q2) && !isinf(q3) && !isinf(q4)) {
// fprintf(pRecFile, "%.7f,%.7f,%.7f,%.7f,",q1,q2,q3,q4);
featureValues.push_back(q1);
featureValues.push_back(q2);
featureValues.push_back(q3);
featureValues.push_back(q4);
} else {
double zero = 0.0;
// fprintf(pRecFile, "%.1f,%.1f,%.1f,%.1f,",zero,zero,zero,zero);;
for(int num =0; num<4; num++)
featureValues.push_back(zero);
}
variableNum+=4;
if(DEBUG_numFeature) printf("current frame : %d\n", variableNum);
}
} else {
// rot mat
double m[3][3];
for (int k =0;k<3;k++) {
m[0][k]= prevFrame[frameCompareTo][j][0]*prevFrame[currentFramePointer][j][0+k]
+ prevFrame[frameCompareTo][j][3]*prevFrame[currentFramePointer][j][3+k]
+ prevFrame[frameCompareTo][j][6]*prevFrame[currentFramePointer][j][6+k];
m[1][k]= prevFrame[frameCompareTo][j][1]*prevFrame[currentFramePointer][j][0+k]
+ prevFrame[frameCompareTo][j][4]*prevFrame[currentFramePointer][j][3+k]
+ prevFrame[frameCompareTo][j][7]*prevFrame[currentFramePointer][j][6+k];
m[2][k]= prevFrame[frameCompareTo][j][2]*prevFrame[currentFramePointer][j][0+k]
+ prevFrame[frameCompareTo][j][5]*prevFrame[currentFramePointer][j][3+k]
+ prevFrame[frameCompareTo][j][8]*prevFrame[currentFramePointer][j][6+k];
}
/*for (int ii = 0;ii<3;ii++)
for(int jj=0;jj<3;jj++)
printf("%d %d %d %.10f\n",frameCompareTo, ii,jj,m[ii][jj]); */
// quat
double q1 = 1.0/2.0 * sqrt(1 + m[0][0] - m[1][1] - m[2][2]);
double q2 = 1.0/(4.0*q1) * (m[0][1] + m[1][0]);
double q3 = 1.0/(4.0*q1) * (m[0][2] + m[2][0]);
double q4 = 1.0/(4.0*q1) * (m[2][1] - m[1][2]);
if (!isnan(q1) && !isnan(q2) && !isnan(q3) && !isnan(q4)
&&!isinf(q1) && !isinf(q2) && !isinf(q3) && !isinf(q4)) {
// fprintf(pRecFile, "%.7f,%.7f,%.7f,%.7f,",q1,q2,q3,q4);
featureValues.push_back(q1);
featureValues.push_back(q2);
featureValues.push_back(q3);
featureValues.push_back(q4);
} else {
double zero = 0.0;
// fprintf(pRecFile, "%.1f,%.1f,%.1f,%.1f,",zero,zero,zero,zero);;
for(int num = 0; num < 4; num++)
featureValues.push_back(zero);
}
variableNum+=4;
if(DEBUG_numFeature) printf("comparing against frame : %d (against:%d)\n", variableNum, compareFrame[i]);
}
} // end j
} // end i
return variableNum;
} // end computeBodyPoseAndMotionInfo
void CelestialNavigationDialog::UpdateFix()
{
std::list<std::vector<double> > J;
std::list<double> R;
double X[3]; /* result */
double initiallat = m_sInitialLatitude->GetValue(), initiallon = m_sInitialLongitude->GetValue();
X[0] = cos(deg2rad(initiallat))*cos(deg2rad(initiallon));
X[1] = cos(deg2rad(initiallat))*sin(deg2rad(initiallon));
X[2] = sin(deg2rad(initiallat));
int iterations = 0;
again:
for (SightList::iterator it = m_SightList.begin(); it != m_SightList.end(); it++) {
Sight *s = *it;
if(!s->IsVisible() || s->m_Type != Sight::ALTITUDE)
continue;
if(s->m_ShiftNm) {
static bool seenwarning = false;
if(!seenwarning) {
wxMessageDialog mdlg(this, _("Shifted sights are not used to compute a fix, \
determine fix visually instead.\n"), wxString(_("Fix Position"), wxID_OK | wxICON_WARNING));
mdlg.ShowModal();
seenwarning = true;
}
continue;
}
double lat, lon;
s->BodyLocation(s->m_DateTime, &lat, &lon, 0, 0);
/* take vector from body location of length equal to
normalized measurement (so the plane this vector
describes intersects the unit sphere along the positions
the sight is valid) */
std::vector<double> v;
double x = cos(deg2rad(lat))*cos(deg2rad(lon));
double y = cos(deg2rad(lat))*sin(deg2rad(lon));
double z = sin(deg2rad(lat));
double sm = sin(deg2rad(s->m_ObservedAltitude));
double cm = cos(deg2rad(s->m_ObservedAltitude));
double d;
switch(m_cbFixAlgorithm->GetSelection()) {
case 0: /* plane */
plane:
/* plane */
v.push_back(x);
v.push_back(y);
v.push_back(z);
d = sm - (X[0]*x + X[1]*y + X[2]*z);
break;
case 1: /* sphere */
{
double xc = X[0] - x, yc = X[1] - y, zc = X[2] - z;
v.push_back(2*xc);
v.push_back(2*yc);
v.push_back(2*zc);
d = cm*cm + (1-sm)*(1-sm) - xc*xc - yc*yc - zc*zc;
} break;
case 2: /* cone */
{
double t2 = X[0]*X[0] + X[1]*X[1] + X[2]*X[2], t = sqrt(t2);
if(t < .1) goto plane;
v.push_back(x);
v.push_back(y);
v.push_back(z);
d = sm - (X[0]*x + X[1]*y + X[2]*z)/t;
} break;
case 3: /* cone 2 */
{
double t2 = X[0]*X[0] + X[1]*X[1] + X[2]*X[2], t = sqrt(t2);
if(t < .1) goto plane;
v.push_back(x/t - x*X[0]*X[0]/(t*t2));
v.push_back(y/t - y*X[1]*X[1]/(t*t2));
v.push_back(z/t - z*X[2]*X[2]/(t*t2));
d = sm - (X[0]*x + X[1]*y + X[2]*z)/t;
} break;
}
J.push_back(v);
R.push_back(d);
}
// return norm(vec)
double
xyz_norm( const xyz_t * const vec)
{
return sqrt(xyz_norm_squared(vec));
}
void CImageDisp::OnLButtonUp(UINT nFlags, CPoint point)
{
// TODO: 여기에 메시지 처리기 코드를 추가 및/또는 기본값을 호출합니다.
SetCurrWindow();
CVisionTab *pVTDlg = (CVisionTab*)GetParent();
if(m_drawRect && !pVTDlg->clip && m_currWnd == RGB_WND) {
m_drawRect=FALSE;
m_clipEnd = point;
// image clipping process
CPoint vertex[4];
vertex[0] = m_clipStart;
vertex[1] = m_clipEnd;
vertex[2] = CPoint(m_clipStart.x, m_clipEnd.y);
vertex[3] = CPoint(m_clipEnd.x, m_clipStart.y);
double dist = 0;
double minV = sqrt((double)(vertex[0].x*vertex[0].x + vertex[0].y*vertex[0].y));
double maxV = sqrt((double)(vertex[1].x*vertex[1].x + vertex[1].y*vertex[1].y));
int imin = 0;
int imax = 0;
for(int i=0; i<sizeof(vertex)/sizeof(CPoint); i++)
{
dist = sqrt((double)(vertex[i].x*vertex[i].x + vertex[i].y*vertex[i].y));
if(dist <= minV) {
minV = dist;
imin = i;
}
if(dist >= maxV) {
maxV = dist;
imax = i;
}
}
m_clipStart = vertex[imin];
m_clipEnd = vertex[imax];
// Control창에 Clip image 좌표 출력
// 변수 값 자체는 현재 picture control 좌표로 가지고 있고
// 출력은 원래 이미지 사이즈(640x480)로 변환하여 출력한다.
CString str;
str.Format("sx: %d, sy: %d, ex: %d, ey: %d",
(int)(m_clipStart.x/m_iZoom), (int)(m_clipStart.y/m_iZoom), (int)(m_clipEnd.x/m_iZoom), (int)(m_clipEnd.y/m_iZoom));
pVTDlg->SetDlgItemTextA(IDC_CLIPCOORDINATE, str);
// Console 출력
/*printf("sx: %d, sy: %d, ex: %d, ey: %d\n",
m_clipStart.x, m_clipStart.y,
m_clipEnd.x, m_clipEnd.y);*/
}
CStatic::OnLButtonUp(nFlags, point);
}
void pulsesequence()
{
/* DECLARE AND LOAD VARIABLES */
char f1180[MAXSTR], /* Flag to start t1 @ halfdwell */
f2180[MAXSTR], /* Flag to start t2 @ halfdwell */
mag_flg[MAXSTR], /* magic-angle coherence transfer gradients */
TROSY[MAXSTR], /* do TROSY on N15 and H1 */
CT_c[MAXSTR],
h1dec[MAXSTR];
int icosel, /* used to get n and p type */
t1_counter=getval("t1_counter"), /* used for states tppi in t1 */
t2_counter=getval("t2_counter"), /* used for states tppi in t2 */
nli = getval("nli"),
nli2 = getval("nli2");
double tau1, /* t1 delay */
tau2, /* t2 delay */
tauCC = 7.0e-3, /* delay for Ca to Cb cosy */
tauC = 13.3e-3, /* constantTime for 13Cb evolution */
timeTN = getval("timeTN"), /* constant time for 15N evolution */
kappa = 5.4e-3,
lambda = 2.4e-3,
zeta = 3.0e-3,
taud = 1.7e-3,
pwClvl = getval("pwClvl"), /* coarse power for C13 pulse */
pwC = getval("pwC"), /* C13 90 degree pulse length at pwClvl */
rf0, /* maximum fine power when using pwC pulses */
/* 90 degree pulse at Cab (46ppm), first off-resonance null at CO (174ppm) */
pwC1, /* 90 degree pulse length on C13 at rf1 */
rf1, /* fine power for 5.1 kHz rf for 600MHz magnet */
/* 180 degree pulse at Ca (46ppm), first off-resonance null at CO(174ppm) */
pwC2, /* 180 degree pulse length at rf2 */
rf2, /* fine power for 11.4 kHz rf for 600MHz magnet */
/* the following pulse lengths for SLP pulses are automatically calculated */
/* by the macro "proteincal". SLP pulse shapes, "offC7" etc are called */
/* directly from your shapelib. */
pwC7 = getval("pwC7"), /*180 degree pulse at CO(174ppm) null at Ca(56ppm) */
pwC7a = getval("pwC7a"), /* pwC7a=pwC7, but not set to zero when pwC7=0 */
phshift7, /* phase shift induced on Cab by pwC7 ("offC7") pulse */
pwZ, /* the largest of pwC7 and 2.0*pwN */
pwZ1, /* the larger of pwC7a and 2.0*pwN for 1D experiments */
rf7, /* fine power for the pwC7 ("offC7") pulse */
/* the following pulse lengths for SLP pulses are automatically calculated */
/* by the macro "proteincal". SLP pulse shapes, "offC5" etc are called */
/* directly from your shapelib. */
pwC5 = getval("pwC5"), /*180 degree pulse at CO(174ppm) null at Ca(56ppm) */
rf5, /* fine power for the pwC7 ("offC7") pulse */
/* g3 inversion pulse in the t1 period (centred at 150ppm) */
pwCgCO_lvl = getval("pwCgCO_lvl"),
pwCgCO = getval("pwCgCO"),
compH = getval("compH"), /* adjustment for C13 amplifier compression */
compC = getval("compC"), /* adjustment for C13 amplifier compression */
pwHs = getval("pwHs"), /* H1 90 degree pulse length at tpwrs */
tpwrs, /* power for the pwHs ("H2Osinc") pulse */
pwHd, /* H1 90 degree pulse length at tpwrd */
tpwrd, /* rf for WALTZ decoupling */
waltzB1 = getval("waltzB1"), /* waltz16 field strength (in Hz) */
pwNlvl = getval("pwNlvl"), /* power for N15 pulses */
pwN = getval("pwN"), /* N15 90 degree pulse length at pwNlvl */
sw1 = getval("sw1"),
sw2 = getval("sw2"),
gstab = getval("gstab"),
gt1 = getval("gt1"), /* coherence pathway gradients */
gzcal = getval("gzcal"), /* g/cm to DAC conversion factor */
gzlvl1 = getval("gzlvl1"),
gzlvl2 = getval("gzlvl2"),
gt0 = getval("gt0"), /* other gradients */
gt3 = getval("gt3"),
gt4 = getval("gt4"),
gt5 = getval("gt5"),
gt7 = getval("gt7"),
gt8 = getval("gt8"),
gzlvl0 = getval("gzlvl0"),
gzlvl3 = getval("gzlvl3"),
gzlvl4 = getval("gzlvl4"),
gzlvl5 = getval("gzlvl5"),
gzlvl6 = getval("gzlvl6"),
gzlvl7 = getval("gzlvl7"),
gzlvl8 = getval("gzlvl8");
getstr("f1180",f1180);
getstr("f2180",f2180);
getstr("mag_flg",mag_flg);
getstr("TROSY",TROSY);
getstr("CT_c",CT_c);
getstr("h1dec",h1dec);
/* LOAD PHASE TABLE */
settable(t2,1,phy);
settable(t3,2,phi3);
settable(t4,1,phx);
settable(t5,4,phi5);
if (TROSY[A]=='y')
{settable(t8,1,phy);
settable(t9,1,phx);
settable(t10,1,phy);
settable(t11,1,phx);
settable(t12,4,recT);}
else
{settable(t8,1,phx);
settable(t9,8,phi9);
settable(t10,1,phx);
settable(t11,1,phy);
settable(t12,4,rec);}
/* INITIALIZE VARIABLES */
if( dpwrf < 4095 )
{ printf("reset dpwrf=4095 and recalibrate C13 90 degree pulse");
psg_abort(1); }
/* maximum fine power for pwC pulses */
rf0 = 4095.0;
/* 90 degree pulse on Cab, null at CO 128ppm away */
pwC1 = sqrt(15.0)/(4.0*128.0*dfrq);
rf1 = (compC*4095.0*pwC)/pwC1;
rf1 = (int) (rf1 + 0.5);
/* 180 degree pulse on Cab, null at CO 128ppm away */
pwC2 = sqrt(3.0)/(2.0*128.0*dfrq);
rf2 = (4095.0*compC*pwC*2.0)/pwC2;
rf2 = (int) (rf2 + 0.5);
if( rf2 > 4095 )
{ printf("Recalibrate so that C13 90 <22us*600/sfrq"); psg_abort(1);}
/* 180 degree one-lobe sinc pulse on CO, null at Ca 118ppm away */
rf7 = (compC*4095.0*pwC*2.0*1.65)/pwC7a; /* needs 1.65 times more */
rf7 = (int) (rf7 + 0.5); /* power than a square pulse */
/* 90 degree one-lobe sinc pulse on CO, null at Ca 118ppm away */
rf5 = (compC*4095.0*pwC*1.69)/pwC5; /* needs 1.69 times more */
rf5 = (int) (rf5 + 0.5); /* power than a square pulse */
/* the pwC7 pulse at the middle of t1 */
if ((nli2 > 0.0) && (nli == 1.0)) nli = 0.0;
if (pwC7a > 2.0*pwN) pwZ = pwC7a; else pwZ = 2.0*pwN;
if ((pwC7==0.0) && (pwC7a>2.0*pwN)) pwZ1=pwC7a-2.0*pwN; else pwZ1=0.0;
if ( nli > 1 ) pwC7 = pwC7a;
if ( pwC7 > 0 ) phshift7 = 320.0;
else phshift7 = 0.0;
/* selective H20 one-lobe sinc pulse */
tpwrs = tpwr - 20.0*log10(pwHs/(compH*pw*1.69)); /* needs 1.69 times more */
tpwrs = (int) (tpwrs); /* power than a square pulse */
/* power level and pulse time for WALTZ 1H decoupling */
pwHd = 1/(4.0 * waltzB1) ;
tpwrd = tpwr - 20.0*log10(pwHd/(compH*pw));
tpwrd = (int) (tpwrd + 0.5);
/* CHECK VALIDITY OF PARAMETER RANGES */
if ( 0.5*nli2*1/(sw2) > timeTN - WFG3_START_DELAY)
{ printf(" nli2 is too big. Make nli2 equal to %d or less.\n",
((int)((timeTN - WFG3_START_DELAY)*2.0*sw2))); psg_abort(1);}
if ( dm[A] == 'y' || dm[B] == 'y' || dm[C] == 'y' )
{ printf("incorrect dec1 decoupler flags! Should be 'nnn' "); psg_abort(1);}
if ( dm2[A] == 'y' || dm2[B] == 'y' )
{ printf("incorrect dec2 decoupler flags! Should be 'nny' "); psg_abort(1);}
if ( dm3[A] == 'y' || dm3[C] == 'y' )
{ printf("incorrect dec3 decoupler flags! Should be 'nyn' or 'nnn' ");
psg_abort(1);}
if ( dpwr2 > 46 )
{ printf("dpwr2 too large! recheck value "); psg_abort(1);}
if ( pw > 20.0e-6 )
{ printf(" pw too long ! recheck value "); psg_abort(1);}
if ( pwN > 100.0e-6 )
{ printf(" pwN too long! recheck value "); psg_abort(1);}
if ( TROSY[A]=='y' && dm2[C] == 'y')
{ text_error("Choose either TROSY='n' or dm2='n' ! "); psg_abort(1);}
/* PHASES AND INCREMENTED TIMES */
/* Phase incrementation for hypercomplex 2D data, States-Haberkorn element */
if (phase1 == 2) { tsadd(t3,1,4); tsadd(t2,1,4);}
if (TROSY[A]=='y')
{ if (phase2 == 2) icosel = +1;
else {tsadd(t4,2,4); tsadd(t10,2,4); icosel = -1;}
}
else { if (phase2 == 2) {tsadd(t10,2,4); icosel = +1;}
else icosel = -1;
}
/* Set up f1180 */
if( ix == 1) d2_init = d2;
tau1 = d2_init + (t1_counter) / sw1;
if((f1180[A] == 'y') && (nli > 1.0))
{ tau1 += ( 1.0 / (2.0*sw1) ); if(tau1 < 0.2e-6) tau1 = 0.0; }
tau1 = tau1/2.0;
/* Set up f2180 */
if( ix == 1) d3_init = d3;
tau2 = d3_init + (t2_counter) / sw2;
if((f2180[A] == 'y') && (nli2 > 1.0))
{ tau2 += ( 1.0 / (2.0*sw2) ); if(tau2 < 0.2e-6) tau2 = 0.0; }
tau2 = tau2/2.0;
/* Calculate modifications to phases for States-TPPI acquisition */
if(t1_counter % 2)
{ tsadd(t3,2,4); tsadd(t12,2,4); }
if(t2_counter % 2)
{ tsadd(t8,2,4); tsadd(t12,2,4); }
/* BEGIN PULSE SEQUENCE */
status(A);
delay(d1);
if (dm3[B] == 'y') lk_hold();
rcvroff();
obspower(tpwr);
decpower(pwClvl);
dec2power(pwNlvl);
decpwrf(rf0);
obsoffset(tof);
txphase(zero);
decphase(zero);
dcplrphase(zero);
delay(1.0e-5);
dec2rgpulse(pwN, zero, 0.0, 0.0); /*destroy N15 and C13 magnetization*/
decrgpulse(pwC, zero, 0.0, 0.0);
zgradpulse(gzlvl0, 0.5e-3);
delay(1.0e-4);
dec2rgpulse(pwN, one, 0.0, 0.0);
decrgpulse(pwC, zero, 0.0, 0.0);
zgradpulse(0.7*gzlvl0, 0.5e-3);
delay(5.0e-4);
rgpulse(pw,zero,0.0,0.0); /* 1H pulse excitation */
dec2phase(zero);
zgradpulse(gzlvl0, gt0);
delay(lambda - gt0);
sim3pulse(2.0*pw, 0.0, 2.0*pwN, zero, zero, zero, 0.0, 0.0);
txphase(one);
zgradpulse(gzlvl0, gt0);
delay(lambda - gt0);
rgpulse(pw, one, 0.0, 0.0);
obspower(tpwrs);
if (TROSY[A]=='y') {
txphase(two);
shaped_pulse("H2Osinc", pwHs, two, 5.0e-4, 0.0);
obspower(tpwr);
zgradpulse(gzlvl3, gt3);
delay(2.0e-4);
dec2rgpulse(pwN, zero, 0.0, 0.0);
delay(0.5*kappa - 2.0*pw);
rgpulse(2.0*pw, two, 0.0, 0.0);
obspower(tpwrd); /* POWER_DELAY */
decphase(zero);
dec2phase(zero);
decpwrf(rf7);
delay(timeTN - 0.5*kappa - POWER_DELAY -WFG_START_DELAY);
}
else {
txphase(zero);
shaped_pulse("H2Osinc", pwHs, zero, 5.0e-4, 0.0);
obspower(tpwrd);
zgradpulse(gzlvl3, gt3);
delay(2.0e-4);
dec2rgpulse(pwN, zero, 0.0, 0.0);
txphase(one);
delay(kappa - pwHd - 2.0e-6 - PRG_START_DELAY);
rgpulse(pwHd,one,0.0,0.0);
txphase(zero);
delay(2.0e-6);
obsprgon("waltz16", pwHd, 90.0); /* PRG_START_DELAY */
xmtron();
decphase(zero);
dec2phase(zero);
decpwrf(rf7);
delay(timeTN - kappa -WFG_START_DELAY);
}
sim3shaped_pulse("","offC7","",0.0, pwC7, 2.0*pwN, zero, zero, zero, 0.0, 0.0);
decphase(t3);
decpwrf(rf5);
delay(timeTN -WFG_STOP_DELAY -pwHd);
dec2rgpulse(pwN, zero, 0.0, 0.0);
if (TROSY[A]=='n') {
xmtroff();
obsprgoff();
rgpulse(pwHd,three,2.0e-6,0.0);
}
delay(2.0e-6);
zgradpulse(gzlvl3, gt3);
delay(2.0e-4);
decpwrf(rf5);
decshaped_pulse("offC5", pwC5, zero, 0.0, 0.0);
delay(2.0e-6);
zgradpulse(-gzlvl7, gt7);
decpwrf(rf0);
decphase(zero);
delay(zeta - gt7 - 0.5*10.933*pwC-2.0e-6);
decrgpulse(pwC*158.0/90.0, zero, 0.0, 0.0);
decrgpulse(pwC*171.2/90.0, two, 0.0, 0.0);
decrgpulse(pwC*342.8/90.0, zero, 0.0, 0.0); /* Shaka 6 composite */
decrgpulse(pwC*145.5/90.0, two, 0.0, 0.0);
decrgpulse(pwC*81.2/90.0, zero, 0.0, 0.0);
decrgpulse(pwC*85.3/90.0, two, 0.0, 0.0);
delay(2.0e-6);
zgradpulse(-gzlvl7, gt7);
decpwrf(rf5);
decphase(one);
txphase(one);
delay(zeta - gt7 - 0.5*10.933*pwC - WFG_START_DELAY-2.0e-6);
/* WFG_START_DELAY */
decshaped_pulse("offC5", pwC5, one, 0.0, 0.0);
delay(2.0e-6);
zgradpulse(1.33*gzlvl3,gt3);
delay(200.0e-6);
if(dm3[B] == 'y'){ /*optional 2H decoupling on */
dec3unblank();
dec3rgpulse(1/dmf3, one, 0.0, 0.0);
dec3unblank();
setstatus(DEC3ch, TRUE, 'w', FALSE, dmf3);
}
decpwrf(rf1);
decphase(t2);
txphase(one);
if (h1dec[A]=='y') {
rgpulse(pwHd,one,0.0,0.0);
txphase(zero);
delay(2.0e-6);
obsprgon("waltz16", pwHd, 90.0); /* PRG_START_DELAY */
xmtron();
}
decrgpulse(pwC1, t3, 0.0, 0.0);
decphase(zero);
decpwrf(rf2);
delay(tauCC -gt5 -202.0e-6 -POWER_DELAY- pwHd -PRG_STOP_DELAY -1/dmf3
-2.0e-6 - WFG_STOP_DELAY);
if(dm3[B] == 'y') { /*optional 2H decoupling off */
dec3rgpulse(1/dmf3, three, 0.0, 0.0);
dec3blank();
setstatus(DEC3ch, FALSE, 'w', FALSE, dmf3);
dec3blank();
}
else delay(1/dmf3 +WFG_STOP_DELAY);
if(h1dec[A]=='y') {
xmtroff();
obsprgoff(); /* PRG_STOP_DELAY */
rgpulse(pwHd,three,2.0e-6,0.0);
}
else delay(pwHd +2.0e-6 +PRG_STOP_DELAY);
delay(2.0e-6);
zgradpulse(-gzlvl5, gt5);
delay(200.0e-6);
decrgpulse(pwC2,zero,0.0,0.0);
delay(2.0e-6);
zgradpulse(-gzlvl5, gt5);
delay(200.0e-6);
decpwrf(rf1);
if(h1dec[A]=='y'){
rgpulse(pwHd,one,0.0,0.0);
txphase(zero);
delay(2.0e-6);
obsprgon("waltz16", pwHd, 90.0); /* PRG_START_DELAY */
xmtron();
}
else delay(pwHd+2.0e-6+PRG_START_DELAY);
if(dm3[B] == 'y'){ /*optional 2H decoupling on */
dec3unblank();
dec3rgpulse(1/dmf3, one, 0.0, 0.0);
dec3unblank();
setstatus(DEC3ch, TRUE, 'w', FALSE, dmf3);
}
else delay(1/dmf3+WFG_START_DELAY);
delay(tauCC -gt5 -202.0e-6 -POWER_DELAY -1/dmf3 -WFG_START_DELAY
-POWER_DELAY -pwHd -2.0e-6 -PRG_START_DELAY
-pwHd-2.0e-6-PRG_STOP_DELAY);
if((h1dec[A]=='y') && (h1dec[B]=='n')) {
xmtroff();
obsprgoff(); /* PRG_STOP_DELAY */
rgpulse(pwHd,one,2.0e-6,0.0);
decrgpulse(pwC1,t2,0.0,0.0);
}
else {
delay(pwHd+2.0e-6+PRG_STOP_DELAY-POWER_DELAY);
if ((h1dec[A]=='y')&&(h1dec[B]=='y')) {
delay(POWER_DELAY);
decrgpulse(pwC1,t2,0.0,0.0);
}
if ((h1dec[A]=='n')&&(h1dec[B]=='n')) {
obspower(tpwr);
simpulse(2.0*pw,pwC1,two,t2,0.0,0.0); /* Assuming 2.0*pw < pwC1 */
}
}
/* It could be h1dec='ny' ??? */
/* xxxxxxxxxxxxxxxxxxxxxx 13Cb EVOLUTION xxxxxxxxxxxxxxxxxx */
if (CT_c[0]=='n') {
if ((nli>1.0) && (tau1>0.0)) { /* total 13C evolution equals d2 exactly */
/* 2.0*pwC1/PI compensates for evolution at 64% rate during pwC1 */
decpwrf(rf7);
if(tau1 - 2.0*pwC1/PI - WFG3_START_DELAY - 0.5*pwZ > 0.0) {
delay(tau1 - 2.0*pwC1/PI - WFG3_START_DELAY - 0.5*pwZ);
/* WFG3_START_DELAY */
sim3shaped_pulse("", "offC7", "", 0.0, pwC7a, 2.0*pwN, zero, zero, zero,
0.0, 0.0);
initval(phshift7, v7);
decstepsize(1.0);
dcplrphase(v7); /* SAPS_DELAY */
delay(tau1 - 2.0*pwC1/PI - SAPS_DELAY - 0.5*pwZ - 2.0e-6);
}
else {
initval(180.0, v7);
decstepsize(1.0);
dcplrphase(v7); /* SAPS_DELAY */
delay(2.0*tau1 - 4.0*pwC1/PI - SAPS_DELAY - 2.0e-6);
}
}
else if (nli==1.0) { /* special 1D check of pwC7 phase enabled when nli=1 */
decpwrf(rf7);
delay(10.0e-6 + SAPS_DELAY + 0.5*pwZ1 + WFG_START_DELAY);
/* WFG3_START_DELAY */
sim3shaped_pulse("", "offC7", "", 0.0, pwC7, 2.0*pwN, zero, zero, zero,
2.0e-6, 0.0);
initval(phshift7, v7);
decstepsize(1.0);
dcplrphase(v7); /* SAPS_DELAY */
delay(10.0e-6 + WFG3_START_DELAY + 0.5*pwZ1);
}
else{ /* 13Ca evolution refocused for 1st increment */
decpwrf(rf2);
decrgpulse(pwC2, zero, 2.0e-6, 0.0);
}
} /* H1 dec. and H2 dec. status are not changed through nonCT evolution*/
else { /* 13C CONSTANT TIME EVOLUTION */
decpwrf(rf0);
decpower(pwCgCO_lvl);
if(h1dec[B]=='y') {
if(tau1 - 2.0*pwC1/PI -WFG_START_DELAY -2*POWER_DELAY> 0.0)
delay(tau1 - 2.0*pwC1/PI -WFG_START_DELAY - 2*POWER_DELAY);
decshaped_pulse("CgCO1",pwCgCO,zero,0.0,0.0);
delay(tauC -gt8 -202.0e-6 -pwHd -2.0e-6 -PRG_STOP_DELAY
-pwCgCO -pwC2 -WFG_STOP_DELAY-1/dmf3);
if(dm3[B] == 'y') { /*optional 2H decoupling off */
dec3rgpulse(1/dmf3, three, 0.0, 0.0);
dec3blank();
setstatus(DEC3ch, FALSE, 'w', FALSE, dmf3);
dec3blank();
}
else delay(1/dmf3+WFG_STOP_DELAY);
xmtroff();
obsprgoff(); /* PRG_STOP_DELAY */
rgpulse(pwHd,three,2.0e-6,0.0);
}
if ((h1dec[B]=='n')&&(dm3[B]=='n')) {
obspower(tpwr);
if(tau1 - 2.0*pwC1/PI -WFG_START_DELAY -3*POWER_DELAY> 0.0) {
delay(tau1 - 2.0*pwC1/PI -WFG3_START_DELAY - 3*POWER_DELAY);
simshaped_pulse("","CgCO1",2.0*pw,pwCgCO,two,zero,0.0,0.0);
}
else simshaped_pulse("","CgCO1",2.0*pw,pwCgCO,two,zero,0.0,0.0);
obspower(tpwrd);
delay(tauC -gt8 -202.0e-6 -pwCgCO -pwC2 -POWER_DELAY);
}
if ((h1dec[B]=='n')&&(dm3[B]=='y')) {
obspower(tpwr);
if(tau1 - 2.0*pwC1/PI - WFG_START_DELAY -3*POWER_DELAY> 0.0) {
delay(tau1 - 2.0*pwC1/PI - WFG3_START_DELAY - 3*POWER_DELAY);
decshaped_pulse("CgCO1",pwCgCO,zero,0.0,0.0);
}
else decshaped_pulse("CgCO1",pwCgCO,zero,0.0,0.0);
delay(taud-0.5*pwC2-WFG_START_DELAY-WFG_STOP_DELAY-pwCgCO);
rgpulse(2.0*pw,two,0.0,0.0);
obspower(tpwrd);
delay(tauC -taud -gt8 -202e-6 -2.0*pw -POWER_DELAY -1/dmf3
-pwCgCO -pwC2 -WFG_STOP_DELAY);
dec3rgpulse(1/dmf3, three, 0.0, 0.0);
dec3blank();
setstatus(DEC3ch, FALSE, 'w', FALSE, dmf3);
dec3blank();
}
delay(2.0e-6);
zgradpulse(gzlvl8,gt8);
delay(200.0e-6-2*POWER_DELAY);
decpower(pwClvl);decpwrf(rf2);
decrgpulse(pwC2,zero,0.0,0.0);
delay(2.0e-6);
zgradpulse(gzlvl8,gt8);
delay(200.0e-6-2*POWER_DELAY);
decpower(pwCgCO_lvl);decpwrf(rf0);
if(h1dec[A]=='y'){
rgpulse(pwHd,one,0.0,0.0);
txphase(zero);
delay(2.0e-6);
obsprgon("waltz16", pwHd, 90.0); /* PRG_START_DELAY */
xmtron();
}
else delay(pwHd+ 2.0e-6 +PRG_START_DELAY);
if(dm3[B] == 'y'){ /*optional 2H decoupling on */
dec3unblank();
dec3rgpulse(1/dmf3, one, 0.0, 0.0);
dec3unblank();
setstatus(DEC3ch, TRUE, 'w', FALSE, dmf3);
}
else delay(1/dmf3+WFG_START_DELAY);
delay(tauC -tau1 -202.0e-6 -gt8 -pwCgCO -WFG_START_DELAY
-WFG_STOP_DELAY -POWER_DELAY -1/dmf3 -WFG_START_DELAY
-pwHd -2.0e-6 -PRG_START_DELAY);
decshaped_pulse("CgCO2",pwCgCO,zero,0.0,0.0);
} /* END of C13 CONSTANT TIME EVOLUTION */
decphase(one);
decpower(pwClvl);
decpwrf(rf1);
decrgpulse(pwC1, one, 2.0e-6, 0.0);
delay(tauCC - gt5 -202.0e-6 -2.0e-6 -pwHd -PRG_STOP_DELAY
-1/dmf3 -WFG_STOP_DELAY);
if(dm3[B] == 'y') { /*optional 2H decoupling off */
dec3rgpulse(1/dmf3, three, 0.0, 0.0);
dec3blank();
setstatus(DEC3ch, FALSE, 'w', FALSE, dmf3);
dec3blank();
}
else delay(1/dmf3+WFG_STOP_DELAY);
if(h1dec[B]=='y') {
xmtroff();
obsprgoff(); /* PRG_STOP_DELAY */
rgpulse(pwHd,three,2.0e-6,0.0);
}
else delay(2.0e-6+pwHd+PRG_STOP_DELAY);
delay(2.0e-6);
zgradpulse(gzlvl5*1.33, gt5);
delay(200.0e-6-2.0*POWER_DELAY);
decpwrf(rf2);
decphase(zero);
decrgpulse(pwC2, zero, 0.0, 0.0);
delay(2.0e-6);
zgradpulse(gzlvl5*1.33,gt5);
delay(200.0e-6-2.0*POWER_DELAY);
decpwrf(rf1);
decphase(t5);
if(h1dec[A]=='y'){
rgpulse(pwHd,one,0.0,0.0);
txphase(zero);
delay(2.0e-6);
obsprgon("waltz16", pwHd, 90.0); /* PRG_START_DELAY */
xmtron();
}
else delay(pwHd+ 2.0e-6 +PRG_START_DELAY);
if(dm3[B] == 'y'){ /*optional 2H decoupling on */
dec3unblank();
dec3rgpulse(1/dmf3, one, 0.0, 0.0);
dec3unblank();
setstatus(DEC3ch, TRUE, 'w', FALSE, dmf3);
}
else delay(1/dmf3+WFG_START_DELAY);
delay(tauCC - gt5 -202.0e-6 -1/dmf3 -WFG_START_DELAY -2.0e-6 -pwHd
-PRG_START_DELAY);
/*decrgpulse(pwC1, t5, 0.0, 0.0); */
decrgpulse(pwC1, zero, 0.0, 0.0);
decpwrf(rf5);
decshaped_pulse("offC5", pwC5, one, 0.0, 0.0);
delay(zeta - gt7 -202.0e-6 - pwHd -2.0e-6 -PRG_STOP_DELAY
-1/dmf3 -WFG_STOP_DELAY -0.5*10.933*pwC-2.0e-6);
if(dm3[B] == 'y') { /*optional 2H decoupling off */
dec3rgpulse(1/dmf3, three, 0.0, 0.0);
dec3blank();
setstatus(DEC3ch, FALSE, 'w', FALSE, dmf3);
dec3blank();
}
else delay(1/dmf3+WFG_STOP_DELAY);
if(h1dec[A]=='y') {
xmtroff();
obsprgoff(); /* PRG_STOP_DELAY */
rgpulse(pwHd,three,2.0e-6,0.0);
}
else delay(2.0e-6+pwHd+PRG_STOP_DELAY);
delay(2.0e-6);
zgradpulse(-gzlvl7, gt7);
decpwrf(rf0);
decphase(zero);
delay(200.0e-6-2.0*POWER_DELAY);
decrgpulse(pwC*158.0/90.0, zero, 0.0, 0.0);
decrgpulse(pwC*171.2/90.0, two, 0.0, 0.0);
decrgpulse(pwC*342.8/90.0, zero, 0.0, 0.0); /* Shaka 6 composite */
decrgpulse(pwC*145.5/90.0, two, 0.0, 0.0);
decrgpulse(pwC*81.2/90.0, zero, 0.0, 0.0);
decrgpulse(pwC*85.3/90.0, two, 0.0, 0.0);
delay(2.0e-6);
zgradpulse(-gzlvl7, gt7);
delay(200.0e-6);
decpwrf(rf5);
decphase(one);
txphase(one);
if(h1dec[A]=='y'){
rgpulse(pwHd,one,0.0,0.0);
txphase(zero);
delay(2.0e-6);
obsprgon("waltz16", pwHd, 90.0); /* PRG_START_DELAY */
xmtron();
}
else delay(pwHd+ 2.0e-6 +PRG_START_DELAY);
if(dm3[B] == 'y'){ /*optional 2H decoupling on */
dec3unblank();
dec3rgpulse(1/dmf3, one, 0.0, 0.0);
dec3unblank();
setstatus(DEC3ch, TRUE, 'w', FALSE, dmf3);
}
else delay(1/dmf3+WFG_START_DELAY);
delay(zeta - gt7 - 0.5*10.933*pwC - WFG_START_DELAY-2.0e-6
-1/dmf3 -WFG_START_DELAY -pwHd -2.0e-6 -PRG_START_DELAY);
/* WFG_START_DELAY */
decshaped_pulse("offC5", pwC5, t5, 0.0, 0.0);
/* xxxxxxxxxxxxxxxxxx OPTIONS FOR N15 EVOLUTION xxxxxxxxxxxxxxxxxxxxx */
dec2phase(t8);
txphase(one);
dcplrphase(zero);
obspower(tpwrd);
if(dm3[B] == 'y') { /*optional 2H decoupling off */
dec3rgpulse(1/dmf3, three, 0.0, 0.0);
dec3blank();
setstatus(DEC3ch, FALSE, 'w', FALSE, dmf3);
dec3blank();
}
if(h1dec[A]=='y') {
xmtroff();
obsprgoff(); /* PRG_STOP_DELAY */
rgpulse(pwHd,three,2.0e-6,0.0);
}
zgradpulse(gzlvl4, gt4);
delay(2.0e-4);
if (TROSY[A]=='n') {
rgpulse(pwHd,one,0.0,0.0);
txphase(zero);
delay(2.0e-6);
obsprgon("waltz16", pwHd, 90.0);
xmtron();
}
dec2rgpulse(pwN, t8, 0.0, 0.0);
decphase(zero);
dec2phase(t9);
decpwrf(rf7);
delay(timeTN - tau2);
sim3shaped_pulse("","offC7","",0.0, pwC7, 2.0*pwN, zero, zero, t9, 0.0, 0.0);
dec2phase(t10);
decpwrf(rf5);
if (TROSY[A]=='y')
{ if (tau2 > gt1 + 2.0*GRADIENT_DELAY + 1.5e-4 + pwHs)
{
txphase(three);
delay(timeTN - pwC2) ; /* WFG_START_DELAY */
decrgpulse(pwC2, zero, 0.0, 0.0);
delay(tau2 - gt1 - 2.0*GRADIENT_DELAY - 1.5e-4 - pwHs);
if (mag_flg[A]=='y') magradpulse(icosel*gzcal*gzlvl1, gt1);
else zgradpulse(icosel*gzlvl1, gt1); /* 2.0*GRADIENT_DELAY */
obspower(tpwrs); /* POWER_DELAY */
delay(1.0e-4 - POWER_DELAY);
shaped_pulse("H2Osinc", pwHs, three, 0.0, 0.0);
txphase(t4);
obspower(tpwr); /* POWER_DELAY */
delay(0.5e-4 - POWER_DELAY);
}
else if (tau2 > pwHs + 0.5e-4)
{
txphase(three);
delay(timeTN-pwC2-gt1-2.0*GRADIENT_DELAY-1.0e-4);
if (mag_flg[A]=='y') magradpulse(icosel*gzcal*gzlvl1, gt1);
else zgradpulse(icosel*gzlvl1, gt1); /* 2.0*GRADIENT_DELAY */
obspower(tpwrs); /* POWER_DELAY */
delay(1.0e-4 - POWER_DELAY); /* WFG_START_DELAY */
decrgpulse(pwC2, zero, 0.0, 0.0);
delay(tau2 - pwHs - 0.5e-4);
shaped_pulse("H2Osinc", pwHs, three, 0.0, 0.0);
txphase(t4);
obspower(tpwr); /* POWER_DELAY */
delay(0.5e-4 - POWER_DELAY);
}
else
{
txphase(three);
delay(timeTN - pwC2 - gt1 - 2.0*GRADIENT_DELAY
- 1.5e-4 - pwHs);
if (mag_flg[A]=='y') magradpulse(icosel*gzcal*gzlvl1, gt1);
else zgradpulse(icosel*gzlvl1, gt1); /* 2.0*GRADIENT_DELAY */
obspower(tpwrs); /* POWER_DELAY */
delay(1.0e-4 - POWER_DELAY); /* WFG_START_DELAY */
shaped_pulse("H2Osinc", pwHs, three, 0.0, 0.0);
txphase(t4);
obspower(tpwr); /* POWER_DELAY */
delay(0.5e-4 - POWER_DELAY);
decrgpulse(pwC2, zero, 0.0, 0.0);
delay(tau2);
}
}
else
{
if (tau2 > kappa)
{
delay(timeTN - pwC2); /* WFG_START_DELAY */
decrgpulse(pwC2, zero, 0.0, 0.0);
delay(tau2 - kappa - PRG_STOP_DELAY - pwHd - 2.0e-6);
xmtroff();
obsprgoff(); /* PRG_STOP_DELAY */
rgpulse(pwHd,three,2.0e-6,0.0);
txphase(t4);
delay(kappa - gt1 - 2.0*GRADIENT_DELAY - 1.0e-4);
if (mag_flg[A]=='y') magradpulse(icosel*gzcal*gzlvl1, gt1);
else zgradpulse(icosel*gzlvl1, gt1); /* 2.0*GRADIENT_DELAY */
obspower(tpwr); /* POWER_DELAY */
delay(1.0e-4 - POWER_DELAY);
}
else if (tau2 > (kappa - pwC2))
{
delay(timeTN + tau2 - kappa - PRG_STOP_DELAY - pwHd - 2.0e-6);
xmtroff();
obsprgoff(); /* PRG_STOP_DELAY */
rgpulse(pwHd,three,2.0e-6,0.0);
txphase(t4); /* WFG_START_DELAY */
decrgpulse(pwC2, zero, 0.0, 0.0);
delay(kappa -pwC2 -gt1 -2.0*GRADIENT_DELAY -1.0e-4);
if (mag_flg[A]=='y') magradpulse(icosel*gzcal*gzlvl1, gt1);
else zgradpulse(icosel*gzlvl1, gt1); /* 2.0*GRADIENT_DELAY */
obspower(tpwr); /* POWER_DELAY */
delay(1.0e-4 - POWER_DELAY);
}
else if (tau2 > gt1 + 2.0*GRADIENT_DELAY + 1.0e-4)
{
delay(timeTN + tau2 - kappa - PRG_STOP_DELAY - pwHd - 2.0e-6);
xmtroff();
obsprgoff(); /* PRG_STOP_DELAY */
rgpulse(pwHd,three,2.0e-6,0.0);
txphase(t4);
delay(kappa - tau2 - pwC2 ); /* WFG_START_DELAY */
decrgpulse(pwC2, zero, 0.0, 0.0);
delay(tau2 - gt1 - 2.0*GRADIENT_DELAY - 1.0e-4);
if (mag_flg[A]=='y') magradpulse(icosel*gzcal*gzlvl1, gt1);
else zgradpulse(icosel*gzlvl1, gt1); /* 2.0*GRADIENT_DELAY */
obspower(tpwr); /* POWER_DELAY */
delay(1.0e-4 - POWER_DELAY);
}
else
{
delay(timeTN + tau2 - kappa - PRG_STOP_DELAY - pwHd - 2.0e-6);
xmtroff();
obsprgoff(); /* PRG_STOP_DELAY */
rgpulse(pwHd,three,2.0e-6,0.0);
txphase(t4);
delay(kappa-tau2-pwC2-gt1-2.0*GRADIENT_DELAY-1.0e-4);
if (mag_flg[A]=='y') magradpulse(icosel*gzcal*gzlvl1, gt1);
else zgradpulse(icosel*gzlvl1, gt1); /* 2.0*GRADIENT_DELAY */
obspower(tpwr); /* POWER_DELAY */
delay(1.0e-4 - POWER_DELAY); /* WFG_START_DELAY */
decrgpulse(pwC2, zero, 0.0, 0.0);
delay(tau2);
}
}
/* xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx */
if (TROSY[A]=='y') rgpulse(pw, t4, 0.0, 0.0);
else sim3pulse(pw, 0.0, pwN, t4, zero, t10, 0.0, 0.0);
txphase(zero);
dec2phase(zero);
zgradpulse(gzlvl5, gt5);
if (TROSY[A]=='y') delay(lambda - 0.65*(pw + pwN) - gt5);
else delay(lambda - 1.3*pwN - gt5);
sim3pulse(2.0*pw, 0.0, 2.0*pwN, zero, zero, zero, 0.0, 0.0);
zgradpulse(gzlvl5, gt5);
txphase(one);
dec2phase(t11);
delay(lambda - 1.3*pwN - gt5);
sim3pulse(pw, 0.0, pwN, one, zero, t11, 0.0, 0.0);
txphase(zero);
dec2phase(zero);
zgradpulse(gzlvl6, gt5);
delay(lambda - 1.3*pwN - gt5);
sim3pulse(2.0*pw, 0.0, 2.0*pwN, zero, zero, zero, 0.0, 0.0);
dec2phase(t10);
zgradpulse(gzlvl6, gt5);
if (TROSY[A]=='y') delay(lambda - 1.6*pwN - gt5);
else delay(lambda - 0.65*pwN - gt5);
if (TROSY[A]=='y') dec2rgpulse(pwN, t10, 0.0, 0.0);
else rgpulse(pw, zero, 0.0, 0.0);
delay((gt1/10.0) + gstab - 0.5*pw + 2.0*GRADIENT_DELAY + POWER_DELAY);
rgpulse(2.0*pw, zero, 0.0, rof1);
dec2power(dpwr2); /* POWER_DELAY */
if (mag_flg[A] == 'y') magradpulse(gzcal*gzlvl2, gt1/10.0);
else zgradpulse(gzlvl2, gt1/10.0); /* 2.0*GRADIENT_DELAY */
statusdelay(C,gstab- rof1);
if (dm3[B]=='y') lk_sample();
setreceiver(t12);
}
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
#endif //ENABLE_AUTO_BED_LEVELING
{
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head);
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
while(block_buffer_tail == next_buffer_head)
{
manage_heater();
manage_inactivity();
lcd_update();
}
#ifdef ENABLE_AUTO_BED_LEVELING
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#endif // ENABLE_AUTO_BED_LEVELING
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[4];
target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
#ifdef LASER
target[E_AXIS] = lround(sqrt(x*x+y*y)*laserRasterPpm); // |distance| * resolution
#else
target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
#endif
#ifdef PREVENT_DANGEROUS_EXTRUDE
if(target[E_AXIS]!=position[E_AXIS])
{
if(degHotend(active_extruder)<extrude_min_temp)
{
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
}
#ifdef PREVENT_LENGTHY_EXTRUDE
if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
{
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
}
#endif
}
#endif
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Mark block as not busy (Not executed by the stepper interrupt)
block->busy = false;
// Number of steps for each axis
#ifndef COREXY
// default non-h-bot planning
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
#else
// corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block->steps_x = labs((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]));
block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]));
#endif
block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
#ifdef LASER
block->steps_e = target[E_AXIS];
#else
block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
block->steps_e *= volumetric_multiplier[active_extruder];
block->steps_e *= extrudemultiply;
block->steps_e /= 100;
#endif // !LASER
block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
// Bail if this is a zero-length block
if (block->step_event_count <= dropsegments)
{
return;
}
block->fan_speed = fanSpeed;
#ifdef BARICUDA
block->valve_pressure = ValvePressure;
block->e_to_p_pressure = EtoPPressure;
#endif
// Compute direction bits for this block
block->direction_bits = 0;
#ifndef COREXY
if (target[X_AXIS] < position[X_AXIS])
{
block->direction_bits |= (1<<X_AXIS);
}
if (target[Y_AXIS] < position[Y_AXIS])
{
block->direction_bits |= (1<<Y_AXIS);
}
#else
if ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= (1<<X_AXIS);
}
if ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= (1<<Y_AXIS);
}
#endif
if (target[Z_AXIS] < position[Z_AXIS])
{
block->direction_bits |= (1<<Z_AXIS);
}
#ifndef LASER
if (target[E_AXIS] < position[E_AXIS])
{
block->direction_bits |= (1<<E_AXIS);
}
#endif // LASER
block->active_extruder = extruder;
//enable active axes
#ifdef COREXY
if((block->steps_x != 0) || (block->steps_y != 0))
{
enable_x();
enable_y();
}
#else
if(block->steps_x != 0) enable_x();
if(block->steps_y != 0) enable_y();
#endif
#ifndef Z_LATE_ENABLE
if(block->steps_z != 0) enable_z();
#endif
#ifndef LASER
// Enable extruder(s)
if(block->steps_e != 0)
{
if (DISABLE_INACTIVE_EXTRUDER) //enable only selected extruder
{
switch(extruder)
{
case 0: enable_e0(); disable_e1(); disable_e2(); break;
case 1: disable_e0(); enable_e1(); disable_e2(); break;
case 2: disable_e0(); disable_e1(); enable_e2(); break;
}
}
else //enable all
{
enable_e0();
enable_e1();
enable_e2();
}
}
if (block->steps_e == 0)
{
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
}
else
{
if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
}
#endif
float delta_mm[4];
#ifndef COREXY
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
#else
delta_mm[X_AXIS] = ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS];
#endif
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
#ifdef LASER
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
#else
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*volumetric_multiplier[active_extruder]*extrudemultiply/100.0;
if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments )
{
block->millimeters = fabs(delta_mm[E_AXIS]);
}
else
{
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
}
#endif // LASER
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
// slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
#ifdef OLD_SLOWDOWN
if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1)
feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5);
#endif
#ifdef SLOWDOWN
// segment time im micro seconds
unsigned long segment_time = lround(1000000.0/inverse_second);
if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5)))
{
if (segment_time < minsegmenttime)
{ // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued));
#ifdef XY_FREQUENCY_LIMIT
segment_time = lround(1000000.0/inverse_second);
#endif
}
}
#endif
// END OF SLOW DOWN SECTION
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
// Calculate and limit speed in mm/sec for each axis
float current_speed[4];
float speed_factor = 1.0; //factor <=1 do decrease speed
for(int i=0; i < 4; i++)
{
current_speed[i] = delta_mm[i] * inverse_second;
if(fabs(current_speed[i]) > max_feedrate[i])
speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
}
// Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT
#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
// Check and limit the xy direction change frequency
unsigned char direction_change = block->direction_bits ^ old_direction_bits;
old_direction_bits = block->direction_bits;
segment_time = lround((float)segment_time / speed_factor);
if((direction_change & (1<<X_AXIS)) == 0)
{
x_segment_time[0] += segment_time;
}
else
{
x_segment_time[2] = x_segment_time[1];
x_segment_time[1] = x_segment_time[0];
x_segment_time[0] = segment_time;
}
if((direction_change & (1<<Y_AXIS)) == 0)
{
y_segment_time[0] += segment_time;
}
else
{
y_segment_time[2] = y_segment_time[1];
y_segment_time[1] = y_segment_time[0];
y_segment_time[0] = segment_time;
}
long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2]));
long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2]));
long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time);
if(min_xy_segment_time < MAX_FREQ_TIME)
speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
#endif
// Correct the speed
if( speed_factor < 1.0)
{
for(unsigned char i=0; i < 4; i++)
{
current_speed[i] *= speed_factor;
}
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
}
// Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count/block->millimeters;
if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)
{
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else
{
block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
// Limit acceleration per axis
if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
}
block->acceleration = block->acceleration_st / steps_per_mm;
block->acceleration_rate = (long)((float)block->acceleration_st * 16777216.0 / HAL_TIMER_RATE);
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
}
}
}
#endif
// Start with a safe speed
float vmax_junction = max_xy_jerk/2;
float vmax_junction_factor = 1.0;
if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2)
vmax_junction = min(vmax_junction, max_z_jerk/2);
if(fabs(current_speed[E_AXIS]) > max_e_jerk/2)
vmax_junction = min(vmax_junction, max_e_jerk/2);
vmax_junction = min(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction;
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
// if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed;
// }
if (jerk > max_xy_jerk) {
vmax_junction_factor = (max_xy_jerk/jerk);
}
if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) {
vmax_junction_factor= min(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));
}
if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) {
vmax_junction_factor = min(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])));
}
vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
if (block->nominal_speed <= v_allowable) {
block->nominal_length_flag = true;
}
else {
block->nominal_length_flag = false;
}
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
previous_nominal_speed = block->nominal_speed;
#ifdef ADVANCE
// Calculate advance rate
if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
block->advance_rate = 0;
block->advance = 0;
}
else {
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) *
(current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUTION_AREA * EXTRUTION_AREA)*256;
block->advance = advance;
if(acc_dist == 0) {
block->advance_rate = 0;
}
else {
block->advance_rate = advance / (float)acc_dist;
}
}
/*
SERIAL_ECHO_START;
SERIAL_ECHOPGM("advance :");
SERIAL_ECHO(block->advance/256.0);
SERIAL_ECHOPGM("advance rate :");
SERIAL_ECHOLN(block->advance_rate/256.0);
*/
#endif // ADVANCE
calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed,
safe_speed/block->nominal_speed);
// Move buffer head
block_buffer_head = next_buffer_head;
// Update position
memcpy(position, target, sizeof(target)); // position[] = target[]
planner_recalculate();
st_wake_up();
}
/* Subroutine */ int clartg_(complex *f, complex *g, real *cs, complex *sn,
complex *r)
{
/* -- LAPACK auxiliary routine (version 2.0) --
Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
Courant Institute, Argonne National Lab, and Rice University
September 30, 1994
Purpose
=======
CLARTG generates a plane rotation so that
[ CS SN ] [ F ] [ R ]
[ __ ] . [ ] = [ ] where CS**2 + |SN|**2 = 1.
[ -SN CS ] [ G ] [ 0 ]
This is a faster version of the BLAS1 routine CROTG, except for
the following differences:
F and G are unchanged on return.
If G=0, then CS=1 and SN=0.
If F=0 and (G .ne. 0), then CS=0 and SN=1 without doing any
floating point operations.
Arguments
=========
F (input) COMPLEX
The first component of vector to be rotated.
G (input) COMPLEX
The second component of vector to be rotated.
CS (output) REAL
The cosine of the rotation.
SN (output) COMPLEX
The sine of the rotation.
R (output) COMPLEX
The nonzero component of the rotated vector.
=====================================================================
[ 25 or 38 ops for main paths ] */
/* System generated locals */
real r__1, r__2;
doublereal d__1;
complex q__1, q__2, q__3;
/* Builtin functions */
void r_cnjg(complex *, complex *);
double c_abs(complex *), r_imag(complex *), sqrt(doublereal);
/* Local variables */
static real d, f1, f2, g1, g2, fa, ga, di;
static complex fs, gs, ss;
if (g->r == 0.f && g->i == 0.f) {
*cs = 1.f;
sn->r = 0.f, sn->i = 0.f;
r->r = f->r, r->i = f->i;
} else if (f->r == 0.f && f->i == 0.f) {
*cs = 0.f;
r_cnjg(&q__2, g);
d__1 = c_abs(g);
q__1.r = q__2.r / d__1, q__1.i = q__2.i / d__1;
sn->r = q__1.r, sn->i = q__1.i;
d__1 = c_abs(g);
r->r = d__1, r->i = 0.f;
/* SN = ONE
R = G */
} else {
f1 = (r__1 = f->r, dabs(r__1)) + (r__2 = r_imag(f), dabs(r__2));
g1 = (r__1 = g->r, dabs(r__1)) + (r__2 = r_imag(g), dabs(r__2));
if (f1 >= g1) {
q__1.r = g->r / f1, q__1.i = g->i / f1;
gs.r = q__1.r, gs.i = q__1.i;
/* Computing 2nd power */
r__1 = gs.r;
/* Computing 2nd power */
r__2 = r_imag(&gs);
g2 = r__1 * r__1 + r__2 * r__2;
q__1.r = f->r / f1, q__1.i = f->i / f1;
fs.r = q__1.r, fs.i = q__1.i;
/* Computing 2nd power */
r__1 = fs.r;
/* Computing 2nd power */
r__2 = r_imag(&fs);
f2 = r__1 * r__1 + r__2 * r__2;
d = sqrt(g2 / f2 + 1.f);
*cs = 1.f / d;
r_cnjg(&q__3, &gs);
q__2.r = q__3.r * fs.r - q__3.i * fs.i, q__2.i = q__3.r * fs.i +
q__3.i * fs.r;
d__1 = *cs / f2;
q__1.r = d__1 * q__2.r, q__1.i = d__1 * q__2.i;
sn->r = q__1.r, sn->i = q__1.i;
q__1.r = d * f->r, q__1.i = d * f->i;
r->r = q__1.r, r->i = q__1.i;
} else {
q__1.r = f->r / g1, q__1.i = f->i / g1;
fs.r = q__1.r, fs.i = q__1.i;
/* Computing 2nd power */
r__1 = fs.r;
/* Computing 2nd power */
r__2 = r_imag(&fs);
f2 = r__1 * r__1 + r__2 * r__2;
fa = sqrt(f2);
q__1.r = g->r / g1, q__1.i = g->i / g1;
gs.r = q__1.r, gs.i = q__1.i;
/* Computing 2nd power */
r__1 = gs.r;
/* Computing 2nd power */
r__2 = r_imag(&gs);
g2 = r__1 * r__1 + r__2 * r__2;
ga = sqrt(g2);
d = sqrt(f2 / g2 + 1.f);
di = 1.f / d;
*cs = fa / ga * di;
r_cnjg(&q__3, &gs);
q__2.r = q__3.r * fs.r - q__3.i * fs.i, q__2.i = q__3.r * fs.i +
q__3.i * fs.r;
d__1 = fa * ga;
q__1.r = q__2.r / d__1, q__1.i = q__2.i / d__1;
ss.r = q__1.r, ss.i = q__1.i;
q__1.r = di * ss.r, q__1.i = di * ss.i;
sn->r = q__1.r, sn->i = q__1.i;
q__2.r = g->r * ss.r - g->i * ss.i, q__2.i = g->r * ss.i + g->i *
ss.r;
q__1.r = d * q__2.r, q__1.i = d * q__2.i;
r->r = q__1.r, r->i = q__1.i;
}
}
return 0;
/* End of CLARTG */
} /* clartg_ */
static void
bandpass_setup(struct FilterBank *fb,
double rate,
double freq,
double band,
int order
) {
/* must be an even number for the algorithm below */
fb->filter_stages = order;
assert (order > 0 && (order%2) == 0 && order <= MAXORDER);
assert (band > 0);
for (uint32_t i = 0; i < fb->filter_stages; ++i) {
fb->f[i].z[z1] = fb->f[i].z[z2] = 0;
}
const double _wc = 2. * M_PI * freq / rate;
const double _ww = 2. * M_PI * band / rate;
double wl = _wc - (_ww / 2.);
double wu = _wc + (_ww / 2.);
if (wu > M_PI - 1e-9) {
/* limit band to below nyquist */
wu = M_PI - 1e-9;
fprintf(stderr, "spectr.lv2: band f:%9.2fHz (%.2fHz -> %.2fHz) exceeds nysquist (%.0f/2)\n",
freq, freq-band/2, freq+band/2, rate);
fprintf(stderr, "spectr.lv2: shifted to f:%.2fHz (%.2fHz -> %.2fHz)\n",
rate * (wu + wl) *.25 / M_PI,
rate * wl * .5 / M_PI,
rate * wu * .5 / M_PI);
}
if (wl < 1e-9) {
/* this is just for completeness, it cannot happen with spectr.lv2 */
wl = 1e-9;
fprintf(stderr, "spectr.lv2: band f:%9.2fHz (%.2fHz -> %.2fHz) contains sub-bass frequencies\n",
freq, freq-band/2, freq+band/2);
fprintf(stderr, "spectr.lv2: shifted to f:%.2fHz (%.2fHz -> %.2fHz)\n",
rate * (wu + wl) *.25 / M_PI,
rate * wl * .5 / M_PI,
rate * wu * .5 / M_PI);
}
wu *= .5; wl *= .5;
assert (wu > wl);
const double c_a = cos (wu + wl) / cos (wu - wl);
const double c_b = 1. / tan (wu - wl);
const double w = 2. * atan (sqrt (tan (wu) * tan(wl)));
const double c_a2 = c_a * c_a;
const double c_b2 = c_b * c_b;
const double ab_2 = 2. * c_a * c_b;
/* bilinear transform coefficients into z-domain */
for (uint32_t i = 0; i < fb->filter_stages / 2; ++i) {
const double omega = M_PI_2 + (2 * i + 1) * M_PI / (2. * (double)fb->filter_stages);
complex_t p = cos (omega) + _I * sin (omega);
const complex_t c = (1. + p) / (1. - p);
const complex_t d = 2 * (c_b - 1) * c + 2 * (1 + c_b);
complex_t v;
v = (4 * (c_b2 * (c_a2 - 1) + 1)) * c;
v += 8 * (c_b2 * (c_a2 - 1) - 1);
v *= c;
v += 4 * (c_b2 * (c_a2 - 1) + 1);
v = csqrt (v);
const complex_t u0 = ab_2 + creal(v * -1.) + ab_2 * creal(c) + _I * (cimag(v * -1.) + ab_2 * cimag(c));
const complex_t u1 = ab_2 + creal( v) + ab_2 * creal(c) + _I * (cimag( v) + ab_2 * cimag(c));
#define ASSIGN_BP(FLT, PC, odd) \
{ \
const complex_t P = PC; \
(FLT).W[a0] = 1.; \
(FLT).W[a1] = -2 * creal(P); \
(FLT).W[a2] = creal(P) * creal(P) + cimag(P) * cimag(P); \
(FLT).W[b0] = 1.; \
(FLT).W[b1] = (odd) ? -2. : 2.; \
(FLT).W[b2] = 1.; \
}
ASSIGN_BP(fb->f[2*i], u0/d, 0);
ASSIGN_BP(fb->f[2*i+1], u1/d, 1);
#undef ASSIGN_BP
}
/* normalize */
const double cos_w = cos (-w);
const double sin_w = sin (-w);
const double cos_w2 = cos (-2. * w);
const double sin_w2 = sin (-2. * w);
complex_t ch = 1;
complex_t cb = 1;
for (uint32_t i = 0; i < fb->filter_stages; ++i) {
ch *= ((1 + fb->f[i].W[b1] * cos_w) + cos_w2)
+ _I * ((fb->f[i].W[b1] * sin_w) + sin_w2);
cb *= ((1 + fb->f[i].W[a1] * cos_w) + fb->f[i].W[a2] * cos_w2)
+ _I * ((fb->f[i].W[a1] * sin_w) + fb->f[i].W[a2] * sin_w2);
}
const complex_t scale = cb / ch;
fb->f[0].W[b0] *= creal(scale);
fb->f[0].W[b1] *= creal(scale);
fb->f[0].W[b2] *= creal(scale);
#ifdef DEBUG_SPECTR
printf("SCALE (%g, %g)\n", creal(scale), cimag(scale));
for (uint32_t i = 0; i < fb->filter_stages; ++i) {
struct Filter *flt = &fb->f[i];
printf("%d: %g %g %g %+g %+g %+g\n", i,
flt->W[a0], flt->W[a1], flt->W[a2],
flt->W[b0], flt->W[b1], flt->W[b2]);
}
#endif
}
void BlobMesh::update()
{
float curSeconds = ofGetElapsedTimef();
//move the blobs
for( vector<Weight>::iterator cIt = mWeights.begin(); cIt != mWeights.end(); ++cIt )
{
cIt->move(ofVec2f(mMeshWidth,
mMeshHeight));
}
// gl::VboMesh::VertexIter iter = mVboMesh->mapVertexBuffer();
vector<uint32_t> indices;
for( int x = 0; x < mMeshWidth; ++x )
{
for( int y = 0; y < mMeshHeight; ++y )
{
mVertexVals[x][y] *= .5;
for( vector<Weight>::iterator cIt = mWeights.begin(); cIt != mWeights.end(); ++cIt )
{
float val = (cIt->pos - ofVec2f(x,y)).length();
val = 10/(val*val);
mVertexVals[x][y] += val;
}
mVertColors[x+y*mMeshWidth] = ofFloatColor((sin(curSeconds*8+sqrt(mVertexVals[x][y])*100)+1)/2,
(sin(curSeconds + x/100.f)+1)/2,
(sin(curSeconds - y/8.f)+1)/2,
(sin((mVertexVals[x][y])*100)+1)/2);
// ++iter;
}
}
mVboMesh.updateColorData(mVertColors, mMeshWidth*mMeshHeight);
float threshhold = .025f;
for( int x = 0; x < mMeshWidth; ++x )
{
for( int y = 0; y < mMeshHeight; ++y )
{
if(cos(curSeconds+x/5.4 + 10*sin(y/12.1) ) > (-1.5*(sin(curSeconds/15.1))-.7))
if(x+1 < mMeshWidth && y+1 < mMeshHeight )
{
if(mVertexVals[x][y] > threshhold && mVertexVals[x+1][y] > threshhold && mVertexVals[x][y+1] > threshhold)
{
// //indices.push_back(y*mMeshWidth+x);
// //indices.push_back(y*mMeshWidth+x+1);
// //indices.push_back((y+1)*mMeshWidth+x+1);
// indices.push_back(x*mMeshHeight+y);
// indices.push_back((x+1)*mMeshHeight+y+1);
// indices.push_back((x+1)*mMeshHeight+y);
}
if(mVertexVals[x][y] > threshhold && mVertexVals[x+1][y+1] > threshhold && mVertexVals[x][y+1] > threshhold)
{
// indices.push_back(x*mMeshHeight+y);
// indices.push_back(x*mMeshHeight+y+1);
// indices.push_back((x+1)*mMeshHeight+y+1);
// //indices.push_back(y*mMeshWidth+x);
// //indices.push_back((y+1)*mMeshWidth+x+1);
// //indices.push_back((y+1)*mMeshWidth+x);
}
}
// the texture coordinates are mapped to [0,1.0)
// texCoords.push_back( Vec2f( x / (float)VERTICES_X, y / (float)VERTICES_Z ) );
}
}
// mVboMesh->bufferIndices(indices);
}
int main(int argc, char *argv[])
{
int test_num, task_num, len, num_cpus;
char mygroup[FILENAME_MAX], mytaskfile[FILENAME_MAX];
char mysharesfile[FILENAME_MAX], ch;
/* Following variables are to capture parameters from script*/
char *group_num_p, *mygroup_p, *script_pid_p, *num_cpus_p;
char *test_num_p, *task_num_p;
int mygroup_num, /* A number attached with a group*/
fd, /* to open a fifo to synchronize*/
counter = 0; /* To take n number of readings*/
double total_cpu_time, /* Accumulated cpu time*/
delta_cpu_time, /* Time the task could run on cpu(s)*/
prev_cpu_time = 0;
double exp_cpu_time; /* Exp time in % by shares calculation*/
struct rusage cpu_usage;
time_t current_time, prev_time, delta_time;
unsigned int fmyshares, num_tasks;
unsigned int mygroup_shares;
struct sigaction newaction, oldaction;
/* Signal handling for alarm*/
sigemptyset(&newaction.sa_mask);
newaction.sa_handler = signal_handler_alarm;
newaction.sa_flags = 0;
sigaction(SIGALRM, &newaction, &oldaction);
/* Collect the parameters passed by the script */
group_num_p = getenv("GROUP_NUM");
mygroup_p = getenv("MYGROUP");
script_pid_p = getenv("SCRIPT_PID");
num_cpus_p = getenv("NUM_CPUS");
test_num_p = getenv("TEST_NUM");
task_num_p = getenv("TASK_NUM");
/* Check if all of them are valid */
if ((test_num_p != NULL) && (((test_num = atoi(test_num_p)) <= 10) && \
((test_num = atoi(test_num_p)) >= 9))) {
if ((group_num_p != NULL) && (mygroup_p != NULL) && \
(script_pid_p != NULL) && (num_cpus_p != NULL) && \
(task_num_p != NULL)) {
mygroup_num = atoi(group_num_p);
scriptpid = atoi(script_pid_p);
num_cpus = atoi(num_cpus_p);
task_num = atoi(task_num_p);
sprintf(mygroup, "%s", mygroup_p);
} else {
tst_brkm(TBROK, cleanup,
"Invalid other input parameters\n");
}
} else {
tst_brkm(TBROK, cleanup, "Invalid test number passed\n");
}
sprintf(mytaskfile, "%s", mygroup);
sprintf(mysharesfile, "%s", mygroup);
strcat(mytaskfile, "/tasks");
strcat(mysharesfile, "/cpu.shares");
write_to_file(mytaskfile, "a", getpid());
/*
* Let us give the default group 100 shares, as other groups will have
* a multiple of 100 shares.
*/
mygroup_shares = 100;
write_to_file(mysharesfile, "w", mygroup_shares);
fd = open("./myfifo", 0);
if (fd == -1)
tst_brkm(TBROK, cleanup, "Could not open fifo synchronize");
read(fd, &ch, 1); /* Block task here to synchronize */
/*
* We need not calculate the expected % cpu time of this task, as
* neither it is required nor it can be predicted as there are idle
* system tasks (and others too) in this group.
*/
FLAG = 0;
total_shares = 0;
shares_pointer = &total_shares;
len = strlen(path);
if (!strncpy(fullpath, path, len))
tst_brkm(TBROK, cleanup,
"Could not copy directory path %s ", path);
if (scan_shares_files(shares_pointer) != 0)
tst_brkm(TBROK, cleanup,
"From function scan_shares_files in %s ", fullpath);
/* return val -1 in case of function error, else 2 is min share value */
if ((fmyshares = read_shares_file(mysharesfile)) < 2)
tst_brkm(TBROK, cleanup,
"in reading shares files %s ", mysharesfile);
if ((read_file(mytaskfile, GET_TASKS, &num_tasks)) < 0)
tst_brkm(TBROK, cleanup,
"in reading tasks files %s ", mytaskfile);
exp_cpu_time = (double)(fmyshares * 100) / (total_shares * num_tasks);
prev_time = time(NULL); /* Note down the time*/
while (1) {
/*
* Need to run some cpu intensive task, which also
* frequently checks the timer value
*/
double f = 274.345, mytime; /*just a float number for sqrt*/
alarm(TIME_INTERVAL);
timer_expired = 0;
/*
* Let the task run on cpu for TIME_INTERVAL. Time of this
* operation should not be high otherwise we can exceed the
* TIME_INTERVAL to measure cpu usage
*/
while (!timer_expired)
f = sqrt(f * f);
current_time = time(NULL);
/* Duration in case its not exact TIME_INTERVAL*/
delta_time = current_time - prev_time;
getrusage(0, &cpu_usage);
/* total_cpu_time = total user time + total sys time */
total_cpu_time = (cpu_usage.ru_utime.tv_sec +
cpu_usage.ru_utime.tv_usec * 1e-6 +
cpu_usage.ru_stime.tv_sec +
cpu_usage.ru_stime.tv_usec * 1e-6) ;
delta_cpu_time = total_cpu_time - prev_cpu_time;
prev_cpu_time = total_cpu_time;
prev_time = current_time;
/* calculate % cpu time each task gets */
if (delta_time > TIME_INTERVAL)
mytime = (delta_cpu_time * 100) /
(delta_time * num_cpus);
else
mytime = (delta_cpu_time * 100) /
(TIME_INTERVAL * num_cpus);
/* No neeed to print the results */
fprintf(stdout, "Grp:-%3d task-%3d:CPU TIME{calc:-%6.2f(s) i.e."
" %6.2f(%%) exp:-%6.2f(%%)} with %3u shares in %lu (s)"
" INTERVAL\n", mygroup_num, task_num, delta_cpu_time,
mytime, exp_cpu_time, fmyshares, delta_time);
counter++;
if (counter >= NUM_INTERVALS) {
switch (test_num) {
case 9: /* Test 09 */
case 10: /* Test 10 */
exit(0); /* This task is done */
break;
default:
tst_brkm(TBROK, cleanup,
"Invalid test number passed\n");
break;
} /* end switch*/
} /* end if*/
} /* end while*/
} /* end main*/
int PointFloatShapeFeatureExtractor::extractFeatures(const LTKTraceGroup& inTraceGroup,
vector<LTKShapeFeaturePtr>& outFeatureVec)
{
LOG( LTKLogger::LTK_LOGLEVEL_DEBUG) << "Entering " <<
"PointFloatShapeFeatureExtractor::extractFeatures()" << endl;
PointFloatShapeFeature *featurePtr = NULL;
float x,y,deltax;
int numPoints=0; // number of pts
int count=0;
int currentStrokeSize;
float sintheta, costheta,sqsum;
int i;
int numberOfTraces = inTraceGroup.getNumTraces();
if (numberOfTraces == 0 )
{
LOG( LTKLogger::LTK_LOGLEVEL_ERR) << "Error: " <<
EEMPTY_TRACE_GROUP << " : " << getErrorMessage(EEMPTY_TRACE_GROUP)<<
" PointFloatShapeFeatureExtractor::extractFeatures" <<endl;
LTKReturnError(EEMPTY_TRACE_GROUP);
}
LTKTraceVector allTraces = inTraceGroup.getAllTraces();
LTKTraceVector::iterator traceIter = allTraces.begin();
LTKTraceVector::iterator traceEnd = allTraces.end();
//***CONCATENTATING THE STROKES***
for (; traceIter != traceEnd ; ++traceIter)
{
floatVector tempxVec, tempyVec;
(*traceIter).getChannelValues("X", tempxVec);
(*traceIter).getChannelValues("Y", tempyVec);
// Number of points in the stroke
numPoints = numPoints + tempxVec.size();
}
//***THE CONCATENATED FULL STROKE***
floatVector xVec(numPoints);
floatVector yVec(numPoints);
traceIter = allTraces.begin();
traceEnd = allTraces.end();
boolVector penUp;
// Add the penUp here
for (; traceIter != traceEnd ; ++traceIter)
{
floatVector tempxVec, tempyVec;
(*traceIter).getChannelValues("X", tempxVec);
(*traceIter).getChannelValues("Y", tempyVec);
currentStrokeSize = tempxVec.size();
if (currentStrokeSize == 0)
{
LOG( LTKLogger::LTK_LOGLEVEL_ERR) << "Error: " <<
EEMPTY_TRACE << " : " << getErrorMessage(EEMPTY_TRACE) <<
" PointFloatShapeFeatureExtractor::extractFeatures" <<endl;
LTKReturnError(EEMPTY_TRACE);
}
for( int point=0; point < currentStrokeSize ; point++ )
{
xVec[count] = tempxVec[point];
yVec[count] = tempyVec[point];
count++;
if(point == currentStrokeSize - 1 )
{
penUp.push_back(true);
}
else
{
penUp.push_back(false);
}
}
}
//***CONCATENTATING THE STROKES***
vector<float> theta(numPoints);
vector<float> delta_x(numPoints-1);
vector<float> delta_y(numPoints-1);
for(i=0; i<numPoints-1; ++i)
{
delta_x[i]=xVec[i+1]-xVec[i];
delta_y[i]=yVec[i+1]-yVec[i];
}
//Add the controlInfo here
sqsum = sqrt( pow(xVec[0],2)+ pow(yVec[0],2))+ EPS;
sintheta = (1+yVec[0]/sqsum)*PREPROC_DEF_NORMALIZEDSIZE/2;
costheta = (1+xVec[0]/sqsum)*PREPROC_DEF_NORMALIZEDSIZE/2;
featurePtr = new PointFloatShapeFeature(xVec[0],
yVec[0],
sintheta,
costheta,
penUp[0]);
outFeatureVec.push_back(LTKShapeFeaturePtr(featurePtr));
featurePtr = NULL;
for( i=1; i<numPoints; ++i)
{
//Add the controlInfo here
sqsum = sqrt(pow(delta_x[i-1],2) + pow(delta_y[i-1],2))+EPS;
sintheta = (1+delta_y[i-1]/sqsum)*PREPROC_DEF_NORMALIZEDSIZE/2;
costheta = (1+delta_x[i-1]/sqsum)*PREPROC_DEF_NORMALIZEDSIZE/2;
featurePtr = new PointFloatShapeFeature(xVec[i],
yVec[i],
sintheta,
costheta,
penUp[i]);
//***POPULATING THE FEATURE VECTOR***
outFeatureVec.push_back(LTKShapeFeaturePtr(featurePtr));
featurePtr = NULL;
}
LOG( LTKLogger::LTK_LOGLEVEL_DEBUG) << "Exiting " <<
"PointFloatShapeFeatureExtractor::extractFeatures()" << endl;
return SUCCESS;
}
// ベクトルの長さを求める
double Vector2D::length(void) const
{
return sqrt(x*x+y*y);
}
double Complex::abs()
{
return sqrt(a*a + b*b);
}
void Route::RenderSegment( ocpnDC& dc, int xa, int ya, int xb, int yb, ViewPort &VP,
bool bdraw_arrow, int hilite_width )
{
// Get the dc boundary
int sx, sy;
dc.GetSize( &sx, &sy );
// Try to exit early if the segment is nowhere near the screen
wxRect r( 0, 0, sx, sy );
wxRect s( xa, ya, 1, 1 );
wxRect t( xb, yb, 1, 1 );
s.Union( t );
if( !r.Intersects( s ) ) return;
// Clip the line segment to the dc boundary
int x0 = xa;
int y0 = ya;
int x1 = xb;
int y1 = yb;
// If hilite is desired, use a Native Graphics context to render alpha colours
// That is, if wxGraphicsContext is available.....
if( hilite_width ) {
if( Visible == cohen_sutherland_line_clip_i( &x0, &y0, &x1, &y1, 0, sx, 0, sy ) ) {
wxPen psave = dc.GetPen();
wxColour y = GetGlobalColor( _T ( "YELO1" ) );
wxColour hilt( y.Red(), y.Green(), y.Blue(), 128 );
wxPen HiPen( hilt, hilite_width, wxPENSTYLE_SOLID );
dc.SetPen( HiPen );
dc.StrokeLine( x0, y0, x1, y1 );
dc.SetPen( psave );
dc.StrokeLine( x0, y0, x1, y1 );
}
} else {
if( Visible == cohen_sutherland_line_clip_i( &x0, &y0, &x1, &y1, 0, sx, 0, sy ) )
dc.StrokeLine( x0, y0, x1, y1 );
}
if( bdraw_arrow ) {
// Draw a direction arrow
double theta = atan2( (double) ( yb - ya ), (double) ( xb - xa ) );
theta -= PI / 2.;
wxPoint icon[10];
double icon_scale_factor = 100 * VP.view_scale_ppm;
icon_scale_factor = fmin ( icon_scale_factor, 1.5 ); // Sets the max size
icon_scale_factor = fmax ( icon_scale_factor, .10 );
// Get the absolute line length
// and constrain the arrow to be no more than xx% of the line length
double nom_arrow_size = 20.;
double max_arrow_to_leg = .20;
double lpp = sqrt( pow( (double) ( xa - xb ), 2 ) + pow( (double) ( ya - yb ), 2 ) );
double icon_size = icon_scale_factor * nom_arrow_size;
if( icon_size > ( lpp * max_arrow_to_leg ) ) icon_scale_factor = ( lpp * max_arrow_to_leg )
/ nom_arrow_size;
for( int i = 0; i < 7; i++ ) {
int j = i * 2;
double pxa = (double) ( s_arrow_icon[j] );
double pya = (double) ( s_arrow_icon[j + 1] );
pya *= icon_scale_factor;
pxa *= icon_scale_factor;
double px = ( pxa * sin( theta ) ) + ( pya * cos( theta ) );
double py = ( pya * sin( theta ) ) - ( pxa * cos( theta ) );
icon[i].x = (int) ( px ) + xb;
icon[i].y = (int) ( py ) + yb;
}
wxPen savePen = dc.GetPen();
dc.SetPen( *wxTRANSPARENT_PEN );
dc.StrokePolygon( 6, &icon[0], 0, 0 );
dc.SetPen( savePen );
}
}
ErrorCode update_torsional_angle(IndexValue bond, struct ResidueData *residues, IndexValue res, FloatValue *coords)
{
if (residues[res].bonds[bond].type == NOT_SET)
{
return ACCESSING_UNSET_BOND_ERROR;
}
IndexValue atom0, atom1, atom2, atom3;
FloatValue x0, y0, z0;
FloatValue x1, y1, z1;
FloatValue x2, y2, z2;
FloatValue x3, y3, z3;
FloatValue x01, y01, z01;
FloatValue x12, y12, z12;
FloatValue x32, y32, z32;
FloatValue px, py, pz;
FloatValue qx, qy, qz;
FloatValue rx, ry, rz;
FloatValue u, v, u1, v1;
FloatValue a;
atom0 = residues[res].bonds[bond].atom0;
x0 = coords[xcoord_index(atom0)];
y0 = coords[ycoord_index(atom0)];
z0 = coords[zcoord_index(atom0)];
atom1 = residues[res].bonds[bond].atom1;
x1 = coords[xcoord_index(atom1)];
y1 = coords[ycoord_index(atom1)];
z1 = coords[zcoord_index(atom1)];
atom2 = residues[res].bonds[bond].atom2;
x2 = coords[xcoord_index(atom2)];
y2 = coords[ycoord_index(atom2)];
z2 = coords[zcoord_index(atom2)];
atom3 = residues[res].bonds[bond].atom3;
x3 = coords[xcoord_index(atom3)];
y3 = coords[ycoord_index(atom3)];
z3 = coords[zcoord_index(atom3)];
vector_subtract(&x01, &y01, &z01, x0, y0, z0, x1, y1, z1); // v01 = v0 - v1
vector_subtract(&x32, &y32, &z32, x3, y3, z3, x2, y2, z2); // v32 = v3 - v2
vector_subtract(&x12, &y12, &z12, x1, y1, z1, x2, y2, z2); // v12 = v1 - v2
vector_crossprod(&px, &py, &pz, x12, y12, z12, x01, y01, z01); // p = v12 x v01
vector_crossprod(&qx, &qy, &qz, x12, y12, z12, x32, y32, z32); // q = v12 x v32
vector_crossprod(&rx, &ry, &rz, x12, y12, z12, qx, qy, qz); // r = v12 x q
vector_dotprod(&u, qx, qy, qz, qx, qy, qz); // u = q * q
vector_dotprod(&v, rx, ry, rz, rx, ry, rz); // v = r * r
if (u <= 0.0 || v <= 0.0)
{
a = 360.0;
}
else
{
vector_dotprod(&u1, px, py, pz, qx, qy, qz); // u1 = p * q
vector_dotprod(&v1, px, py, pz, rx, ry, rz); // v1 = p * r
u = u1 / sqrt(u);
v = v1 / sqrt(v);
if (fabs(u) > MIN_FLOAT_DIFF || fabs(v) > MIN_FLOAT_DIFF) a = atan2(v, u) * RADIANS_TO_DEGREES;
else a = 360.0;
}
residues[res].bonds[bond].angle = a;
residues[res].bonds[bond].newangle = a;
return NO_ERROR;
}
void
VSpatialFilter(VAttrList list,VDouble fwhm)
{
VAttrListPosn posn;
VImage src[NSLICES],xsrc=NULL,tmp=NULL,dest=NULL,kernel=NULL,tmp2d=NULL;
VString str=NULL;
float v0,v1,v2,v3;
int b,r,c,i,size;
int n,nslices,nrows,ncols,dim;
double u, sigma=0;
extern VImage VGaussianConv (VImage,VImage,VBand,double,int);
/* get image dimensions */
dim = 3;
v0 = v1 = v2 = v3 = 1;
n = i = nrows = ncols = 0;
str = VMalloc(100);
for (VFirstAttr (list, & posn); VAttrExists (& posn); VNextAttr (& posn)) {
if (i >= NSLICES) VError(" too many slices");
if (VGetAttrRepn (& posn) != VImageRepn) continue;
VGetAttrValue (& posn, NULL,VImageRepn, & xsrc);
if (VPixelRepn(xsrc) != VShortRepn) continue;
if (VImageNBands(xsrc) > n) n = VImageNBands(xsrc);
if (VImageNRows(xsrc) > nrows) nrows = VImageNRows(xsrc);
if (VImageNColumns(xsrc) > ncols) ncols = VImageNColumns(xsrc);
if (VGetAttr (VImageAttrList (xsrc), "voxel", NULL,VStringRepn, (VPointer) & str) == VAttrFound) {
sscanf(str,"%f %f %f",&v1,&v2,&v3);
}
src[i] = xsrc;
i++;
}
nslices = i;
/* in general, apply 3D spatial filtering */
dim = 3;
/* only if clearly non-isotropic apply 2D filtering */
v0 = 0.5*(v1+v2);
if (ABS(v0-v3) > 0.5) dim = 2;
/*
** Sigma
*/
sigma = fwhm/sqrt(8.0*log(2.0));
sigma /= (double)v1;
/*
** 2D gauss filtering
*/
if (dim==2) {
fprintf(stderr," 2D spatial filter: fwhm= %.3f mm sigma= %.3f vox\n",fwhm,sigma);
size = (int)(6.0 * sigma + 1.5);
if ((size & 1) == 0) size++;
fprintf(stderr," size= %d\n",size);
for (b=0; b<nslices; b++) {
if (VImageNRows(src[b]) < 2) continue;
tmp2d = VGaussianConv(src[b],tmp2d, VAllBands, sigma, size);
src[b] = VCopyImagePixels(tmp2d,src[b],VAllBands);
}
VDestroyImage(tmp2d);
}
/*
** 3D gauss filtering
*/
if (dim==3) {
fprintf(stderr," 3D spatial filter: fwhm= %.3f mm sigma= %.3f vox\n",fwhm,sigma);
kernel = VSGaussKernel(sigma);
tmp = VCreateImage(nslices,nrows,ncols,VFloatRepn);
VFillImage(tmp,VAllBands,0);
for (i=0; i<n; i++) {
if (i%20 == 0) fprintf(stderr," i= %5d\r",i);
VFillImage(tmp,VAllBands,0);
for (b=0; b<nslices; b++) {
if (VImageNRows(src[b]) < 2) continue;
for (r=0; r<nrows; r++) {
for (c=0; c<ncols; c++) {
u = VPixel(src[b],i,r,c,VShort);
VPixel(tmp,b,r,c,VFloat) = u;
}
}
}
dest = VGauss3d (tmp,dest,kernel);
for (b=0; b<nslices; b++) {
if (VImageNRows(src[b]) < 2) continue;
for (r=0; r<nrows; r++) {
for (c=0; c<ncols; c++) {
u = VPixel(dest,b,r,c,VFloat);
VPixel(src[b],i,r,c,VShort) = u;
}
}
}
}
}
}
double *r8vec_normal_01_new ( int n, int *seed )
/******************************************************************************/
/*
Purpose:
R8VEC_NORMAL_01_NEW returns a unit pseudonormal R8VEC.
Discussion:
The standard normal probability distribution function (PDF) has
mean 0 and standard deviation 1.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
06 August 2013
Author:
John Burkardt
Parameters:
Input, int N, the number of values desired.
Input/output, int *SEED, a seed for the random number generator.
Output, double R8VEC_NORMAL_01_NEW[N], a sample of the standard normal PDF.
Local parameters:
Local, double R[N+1], is used to store some uniform random values.
Its dimension is N+1, but really it is only needed to be the
smallest even number greater than or equal to N.
Local, int X_LO, X_HI, records the range of entries of
X that we need to compute.
*/
{
int i;
int m;
const double pi = 3.141592653589793;
double *r;
double *x;
int x_hi;
int x_lo;
x = ( double * ) malloc ( n * sizeof ( double ) );
/*
Record the range of X we need to fill in.
*/
x_lo = 1;
x_hi = n;
/*
If we need just one new value, do that here to avoid null arrays.
*/
if ( x_hi - x_lo + 1 == 1 )
{
r = r8vec_uniform_01_new ( 2, seed );
x[x_hi-1] = sqrt ( - 2.0 * log ( r[0] ) ) * cos ( 2.0 * pi * r[1] );
free ( r );
}
/*
If we require an even number of values, that's easy.
*/
else if ( ( x_hi - x_lo + 1 ) % 2 == 0 )
{
m = ( x_hi - x_lo + 1 ) / 2;
r = r8vec_uniform_01_new ( 2*m, seed );
for ( i = 0; i <= 2*m-2; i = i + 2 )
{
x[x_lo+i-1] = sqrt ( - 2.0 * log ( r[i] ) ) * cos ( 2.0 * pi * r[i+1] );
x[x_lo+i ] = sqrt ( - 2.0 * log ( r[i] ) ) * sin ( 2.0 * pi * r[i+1] );
}
free ( r );
}
/*
If we require an odd number of values, we generate an even number,
and handle the last pair specially, storing one in X(N).
*/
else
{
x_hi = x_hi - 1;
m = ( x_hi - x_lo + 1 ) / 2 + 1;
r = r8vec_uniform_01_new ( 2*m, seed );
for ( i = 0; i <= 2*m-4; i = i + 2 )
{
x[x_lo+i-1] = sqrt ( - 2.0 * log ( r[i] ) ) * cos ( 2.0 * pi * r[i+1] );
x[x_lo+i ] = sqrt ( - 2.0 * log ( r[i] ) ) * sin ( 2.0 * pi * r[i+1] );
}
i = 2*m - 2;
x[x_lo+i-1] = sqrt ( - 2.0 * log ( r[i] ) ) * cos ( 2.0 * pi * r[i+1] );
free ( r );
}
return x;
}