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
Example #2
0
//2点間の距離を計算
double Vector2D::Distance(const Vector2D &vec) const
{
	return sqrt((vec.x * vec.x) + (vec.y * vec.y));
}
Example #3
0
double dist(pt_t a, pt_t b)
{
    return sqrt(SQR(a.x - b.x) + SQR(a.y - b.y));
}
Example #4
0
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;
}
Example #5
0
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);
}
Example #7
0
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;
}
Example #8
0
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(); 
}
Example #9
0
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;
}
Example #10
0
File: yadro.cpp Project: Ztaz/cpp
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;
        }
    }
}
Example #11
0
/* 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_ */
Example #12
0
File: yadro.cpp Project: Ztaz/cpp
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;
        }
    }
}
Example #13
0
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);
    }
Example #16
0
// return norm(vec)
double
xyz_norm( const xyz_t * const vec)
{
  return sqrt(xyz_norm_squared(vec));
}
Example #17
0
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);
}
Example #18
0
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();
}
Example #20
0
/* 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_ */
Example #21
0
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);
}
Example #23
0
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;
}
Example #25
0
// ベクトルの長さを求める
double Vector2D::length(void) const
{
	return sqrt(x*x+y*y);
}
double Complex::abs()
{
    return sqrt(a*a + b*b);
}
Example #27
0
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 );
    }
}
Example #28
0
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;
}
Example #29
0
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;
}