Esempio n. 1
0
choke me
#endif

void
mpq_set_d (mpq_ptr dest, double d)
{
    int negative;
    mp_exp_t exp;
    mp_limb_t tp[LIMBS_PER_DOUBLE];
    mp_ptr np, dp;
    mp_size_t nn, dn;
    int c;

    DOUBLE_NAN_INF_ACTION (d,
                           __gmp_invalid_operation (),
                           __gmp_invalid_operation ());

    negative = d < 0;
    d = ABS (d);

    exp = __gmp_extract_double (tp, d);

    /* There are two main version of the conversion.  The `then' arm handles
       numbers with a fractional part, while the `else' arm handles integers.  */
#if LIMBS_PER_DOUBLE == 4
    if (exp <= 1 || (exp == 2 && (tp[0] | tp[1]) != 0))
#endif
#if LIMBS_PER_DOUBLE == 3
        if (exp <= 1 || (exp == 2 && tp[0] != 0))
#endif
#if LIMBS_PER_DOUBLE == 2
            if (exp <= 1)
#endif
            {
                if (d == 0.0)
                {
                    SIZ(NUM(dest)) = 0;
                    SIZ(DEN(dest)) = 1;
                    PTR(DEN(dest))[0] = 1;
                    return;
                }

                dn = -exp;
                np = MPZ_NEWALLOC (NUM(dest), 3);
#if LIMBS_PER_DOUBLE == 4
                if ((tp[0] | tp[1] | tp[2]) == 0)
                    np[0] = tp[3], nn = 1;
                else if ((tp[0] | tp[1]) == 0)
                    np[1] = tp[3], np[0] = tp[2], nn = 2;
                else if (tp[0] == 0)
                    np[2] = tp[3], np[1] = tp[2], np[0] = tp[1], nn = 3;
                else
                    np[3] = tp[3], np[2] = tp[2], np[1] = tp[1], np[0] = tp[0], nn = 4;
#endif
#if LIMBS_PER_DOUBLE == 3
                if ((tp[0] | tp[1]) == 0)
                    np[0] = tp[2], nn = 1;
                else if (tp[0] == 0)
                    np[1] = tp[2], np[0] = tp[1], nn = 2;
                else
                    np[2] = tp[2], np[1] = tp[1], np[0] = tp[0], nn = 3;
#endif
#if LIMBS_PER_DOUBLE == 2
                if (tp[0] == 0)
                    np[0] = tp[1], nn = 1;
                else
                    np[1] = tp[1], np[0] = tp[0], nn = 2;
#endif
                dn += nn + 1;
                ASSERT_ALWAYS (dn > 0);
                dp = MPZ_NEWALLOC (DEN(dest), dn);
                MPN_ZERO (dp, dn - 1);
                dp[dn - 1] = 1;
                count_trailing_zeros (c, np[0] | dp[0]);
                if (c != 0)
                {
                    mpn_rshift (np, np, nn, c);
                    nn -= np[nn - 1] == 0;
                    mpn_rshift (dp, dp, dn, c);
                    dn -= dp[dn - 1] == 0;
                }
                SIZ(DEN(dest)) = dn;
                SIZ(NUM(dest)) = negative ? -nn : nn;
            }
            else
            {
                nn = exp;
                np = MPZ_NEWALLOC (NUM(dest), nn);
                switch (nn)
                {
                default:
                    MPN_ZERO (np, nn - LIMBS_PER_DOUBLE);
                    np += nn - LIMBS_PER_DOUBLE;
                    /* fall through */
#if LIMBS_PER_DOUBLE == 2
                case 2:
                    np[1] = tp[1], np[0] = tp[0];
                    break;
#endif
#if LIMBS_PER_DOUBLE == 3
                case 3:
                    np[2] = tp[2], np[1] = tp[1], np[0] = tp[0];
                    break;
                case 2:
                    np[1] = tp[2], np[0] = tp[1];
                    break;
#endif
#if LIMBS_PER_DOUBLE == 4
                case 4:
                    np[3] = tp[3], np[2] = tp[2], np[1] = tp[1], np[0] = tp[0];
                    break;
                case 3:
                    np[2] = tp[3], np[1] = tp[2], np[0] = tp[1];
                    break;
                case 2:
                    np[1] = tp[3], np[0] = tp[2];
                    break;
#endif
                }
                dp = PTR(DEN(dest));
                dp[0] = 1;
                SIZ(DEN(dest)) = 1;
                SIZ(NUM(dest)) = negative ? -nn : nn;
            }
}
Esempio n. 2
0
void psAxesBox(
	float x, float y, float width, float height,
	float x1Beg, float x1End, float p1Beg, float p1End, 
	float d1Num, float f1Num, int n1Tic, int grid1, char *label1,
	float x2Beg, float x2End, float p2Beg, float p2End,
	float d2Num, float f2Num, int n2Tic, int grid2, char *label2,
	char *labelFont, float labelSize,
	char *title, char *titleFont, float titleSize,
	int style)
/*
FUNCTION:  draw an axes box via PostScript

PARAMETERS:
x		i x coordinate of lower left corner of box
y		i y coordinate of lower left corner of box
width		i width of box
height		i height of box
x1Beg  		i axis value at beginning of axis 1
x1End		i axis value at end of axis 1
p1Beg  		i pad value at beginning of axis 1
p1End		i pad value at end of axis 1
d1Num		i numbered tic increment for axis 1 (0.0 for automatic)
f1Num		i first numbered tic for axis 1
n1Tic		i number of horizontal tics per numbered tic for axis 1
grid1		i grid code for axis 1:  NONE, DOT, DASH, or SOLID
label1		i label for axis 1
x2Beg  		i axis value at beginning of axis 2
x2End		i axis value at end of axis 2
p2Beg  		i pad value at beginning of axis 2
p2End		i pad value at end of axis 2
d2Num		i vertical numbered tic increment (0.0 for automatic)
f2Num		i first numbered vertical tic
n2Tic		i number of vertical tics per numbered tic
grid2		i grid code for vertical axis:  NONE, DOT, DASH, or SOLID
label2		i vertical axis label
labelFont	i name of font to use for axes labels
labelSize	i size of font to use for axes labels
title		i axes box title
titleFont	i name of font to use for title
titleSize	i size of font to use for title
style		i NORMAL (axis 1 on bottom, axis 2 on left) 
		  SEISMIC (axis 1 on left, axis 2 on top)

NOTES:
psAxesBox will determine the numbered tic increment and first
numbered tic automatically, if the specified increment is zero.
Axis numbering is in scientific notation, if necessary and is plotted to four significant digits.

Pad values must be specified in the same units as the corresponding 
axes values.  These pads are useful when the contents of the axes box 
requires more space than implied by the axes values.  For example, 
the first and last seismic wiggle traces plotted inside an axes box 
will typically extend beyond the axes values corresponding to the 
first and last traces.  However, all tics will lie with the limits 
specified in the axes values (x1Beg, x1End, x2Beg, x2End).

AUTHOR:   Dave Hale,  Colorado School of Mines, 06/27/89
MODIFIED: Ken Larner, Colorado School of Mines, 08/30/90
*/
{
	int n1num,n2num,ntic,ndash,grided;
	int ndig,ndigits,nexp,nexpmax,nexplot1,nexplot2,
		nplaces,nplacesmax,nformat;
	float xa,xas,ya,yas,yb,xb,ticsize,dnum,fnum,dtic,amin,amax,azero,
		base,scale,anum,anumnorm,atic,fnexp,
		size1,size2,ticb,numb,numb2,labelb,dash[2],
		labelCH,labelCW,labelCA,labelCD,
		titleCH,titleCW,titleCA,titleCD,
		pnorm,fdexp,azeronorm;
	char str[256],str2[256],sformat[256];

	/* determine font dimensions */
	labelCH = fontheight(labelFont,labelSize);
	labelCW = fontwidth(labelFont,labelSize);
	labelCA = fontascender(labelFont,labelSize);
	labelCD = fontdescender(labelFont,labelSize);
	titleCH = fontheight(titleFont,titleSize);
	titleCW = fontwidth(titleFont,titleSize);
	titleCA = fontascender(titleFont,titleSize);
	titleCD = fontdescender(titleFont,titleSize);

	/* determine sizes of axes 1 and 2 */
	if (style==NORMAL) {
		size1 = width;
		size2 = height;
	} else {
		size1 = height;
		size2 = width;
	}

	/* determine numbered tic intervals */
	if (d1Num==0.0) {
		n1num = size1/(4*labelCW);
		scaxis(x1Beg,x1End,&n1num,&d1Num,&f1Num);
	}
	if (d2Num==0.0) {
		n2num = size2/(4*labelCW);
		scaxis(x2Beg,x2End,&n2num,&d2Num,&f2Num);
	}

	/* save graphics state */
	gsave();

	/* set gray (for black axes) */
	setgray(0.0);

	/* translate coordinate system, so that origin is at x,y */
	translate(x,y);

	/* if style is not NORMAL, rotate coordinate system */
	if (style!=NORMAL) {
		rotate(-90.0);
		translate(-height,0.0);
	}

	/* start a new path (just to be safe) */
	newpath();

	/* set font and character size */
	setfont(labelFont,labelSize);

	/* determine tic size */
	ticsize = 0.3*labelCH;

	/* draw axis 1 */
	amin = (x1Beg<x1End)?x1Beg:x1End;
	amax = (x1Beg>x1End)?x1Beg:x1End;
	azero = 0.0001*(amax-amin);
	dnum = d1Num;  fnum = f1Num;  ntic = n1Tic;
	scale = size1/(x1End+p1End-x1Beg-p1Beg);
	base = -scale*(x1Beg+p1Beg);
	ticb = -ticsize;
	numb = 1.2*ticb-labelCA;
	if(style!=NORMAL)   numb = 1.2*ticb;
	labelb = numb-labelCH;

	/* determine axes1 exponent for scientific notation   */
	ndigits = 0;
	ndig    = 0;
	fdexp = log10(1.001*ABS(dnum));
	if(fdexp<0.)   fdexp -=1.0;
	for (anum=fnum; anum<=amax; anum+=dnum) {
		if(anum==fnum && anum==0.)
			nexpmax = 0;
		nexp    = 0;
		if(anum!=0.)   {
			fnexp = log10(1.001*ABS(anum));
			if(fnexp>0.)   nexp    = (int)fnexp;
			else           nexp    = (int)fnexp-1;		
			if(anum==fnum)
				nexpmax = nexp;
			if(nexpmax<nexp || nexpmax==0)    
				nexpmax = nexp;
			ndig = 1+nexp-(int)fdexp;
		}
		if(ndigits<ndig)   ndigits = ndig;
	}
	nexplot1 = 0;
	if(ABS(nexpmax)>3)   
		nexplot1 = nexpmax;
	if((nexpmax<0) && (ndigits+ABS(nexpmax))>4)   
		nexplot1 = nexpmax;
	
	/* loop for axis1 numbering */
	nplacesmax = 0;
	pnorm      = pow(10.0,(double)nexplot1);
	azeronorm  = azero/pnorm;
	fdexp = log10(1.001*ABS(dnum/pnorm));
	if(fdexp<0.)   fdexp -=1.0;
	for (anum=fnum; anum<=amax; anum+=dnum) {

		if (anum<amin) continue;
		xa = base+scale*anum;
		xas = xa+0.25*labelCH;
		moveto(xa,0.0);  lineto(xa,ticb);
		anumnorm = anum/pnorm;

		/* find the number of places in axis1 numbers */
		nplaces = 1;
		nexp    = 1;
		nexp    = 0;
		if (anumnorm<-azeronorm || anumnorm>azeronorm)   {
			fnexp = log10(1.001*ABS(anumnorm));
			if(fnexp>0.)   {
				nexp    = (int)fnexp;
				nplaces = nexp+2;
			}
			else   {
				nexp    = (int)fnexp-1;
				nplaces = -nexp+2;
			}
		}
		/* numbers limited to four significant digits */
		ndigits = 1+nexp-(int)fdexp;
		if(ndigits>4)   ndigits = 4;

		if((nexp>=0) && ((ndigits+1)>nplaces))    
			nplaces = ndigits+1;
		if(anum<0. )   
			nplaces +=1;
		if(nexp<0)   
			nplaces = nplaces+ndigits-1;
		if(nplacesmax<nplaces)   
			nplacesmax = nplaces;

		nformat = ndigits-(nexp+1);
		if(nformat<0)   nformat = 0;

		if (anumnorm>-azeronorm && anumnorm<azeronorm)
			sprintf(str,"%.4g",0.0);
		else	{
			sprintf(sformat,"%%.%df",nformat);
			sprintf(str,sformat,anumnorm);
		}
	
		if (style!=NORMAL) {
			moveto(xas,numb);
			rotate(90.0);
			justshow(-1.0,str);
			rotate(-90.0);
		}
		else   {
			moveto(xa,numb);
			justshow(-0.5,str);
		}
			
	}
	/* draw exponential multiplier for axis1 */
	if(nexplot1!=0)  {
		if(style==NORMAL)   {
			moveto(size1,labelb);
			show("x10");
			moveto(size1+1.5*labelCW,labelb+0.5*labelCA);
			sprintf(str2,"%d",nexplot1);
			show(str2);
		}
		else   {
			moveto(size1+1.4*labelCH,-2.0*labelCW);
			rotate(90.);
			show("x10");
			rotate(-90.);
			moveto(size1+1.05*labelCH,-0.5*labelCW);
			sprintf(str2,"%d",nexplot1);
			rotate(90.);
			show(str2);
			rotate(-90.);
		}
	}
	stroke();

	/* draw axis1 tick marks */
	if(style!=NORMAL)   labelb = numb-0.5*nplacesmax*labelCW;
	dtic = dnum/ntic;
	for (atic=fnum-ntic*dtic-dtic; atic<=amax; atic+=dtic) {
		if (atic<amin) continue;
		xa = base+scale*atic;
		moveto(xa,0.0);  lineto(xa,ticb/2);
	}
	stroke();

	/* draw axis1 grid lines */
	if (grid1==SOLID) {
		grided = 1;
		ndash = 0;
	} else if (grid1==DASH) {
		grided = 1;
		ndash = 1;  dash[0] = 10;
	} else if (grid1==DOT) {
		grided = 1;
		ndash = 2;  dash[0] = 1;  dash[1] = 5;
	} else
		grided = 0;
	if (grided) {
		for (anum=fnum; anum<=amax; anum+=dnum) {
			if (anum<amin) continue;
			xa = base+scale*anum;
			moveto(xa,0.0);  lineto(xa,size2);
		}
		setdash(dash,ndash,0.0);
		stroke();
		setdash(dash,0,0.0);
	}
	
	/* draw axis1 label */
	moveto(size1/2.0,labelb);
	if(style!=NORMAL)   {
		rotate(180.);
		justshow(-0.5,label1);
		rotate(-180.);
	}
	else 	justshow(-0.5,label1);
 
	/* draw axis 2 */
	amin = (x2Beg<x2End)?x2Beg:x2End;
	amax = (x2Beg>x2End)?x2Beg:x2End;
	azero = 0.0001*(amax-amin);
	dnum = d2Num;  fnum = f2Num;  ntic = n2Tic;
	scale = size2/(x2End+p2End-x2Beg-p2Beg);
	base = -scale*(x2Beg+p2Beg);
	ticb = -ticsize;
	numb = ticb+labelCD;
	labelb = numb-labelCH;
	
	/* determine axes2 exponent for scientific notation   */
	ndigits = 0;
	ndig    = 0;
	fdexp   = log10(1.001*ABS(dnum));
	if(fdexp<0.)   fdexp -=1.0;
	for (anum=fnum; anum<=amax; anum+=dnum) {
		if(anum==fnum && anum==0.)
			nexpmax = 0;
		nexp    = 0;
		if(anum!=0.)   {
			fnexp = log10(1.001*ABS(anum));
			if(fnexp>0.)   nexp    = (int)fnexp;
			else           nexp    = (int)fnexp-1;		
			if(anum==fnum)
				nexpmax = nexp;
			if(nexpmax<nexp || nexpmax==0)    
				nexpmax = nexp;
			ndig = 1+nexp-(int)fdexp;
		}
		if(ndigits<ndig)   ndigits = ndig;
	}
	nexplot2 = 0;
	if(ABS(nexpmax)>3)   
		nexplot2 = nexpmax;
	if((nexpmax<0) && (ndigits+ABS(nexpmax))>4)   
		nexplot2 = nexpmax;
	
	/* loop for axis 2 numbering */
	nplacesmax = 0;
	pnorm      = pow(10.0,(double)nexplot2);
	azeronorm  = azero/pnorm;
	fdexp = log10(1.001*ABS(dnum/pnorm));
	if(fdexp<0.)   fdexp -=1.0;
	for (anum=fnum; anum<=amax; anum+=dnum) {
		ya = base+scale*anum;
		moveto(0.0,ya);  lineto(ticb,ya);
		anumnorm = anum/pnorm;
		yas = ya-0.25*labelCH;
	
		/* find the number of places in axis2 numbers */
		nplaces = 1;
		nexp    = 1;
		if (anumnorm<-azeronorm || anumnorm>azeronorm)   {
			fnexp = log10(1.001*ABS(anumnorm));
			if(fnexp>0.)   {
				nexp    = (int)fnexp;
				nplaces = nexp+2;
			}
			else   {
				nexp    = (int)fnexp-1;
				nplaces = -nexp+2;
			}
		}
		/* numbers limited to four significant digits */
		ndigits = 1+nexp-(int)fdexp;
		if(ndigits>4)   ndigits = 4;

		if((nexp>=0) && ((ndigits+1)>nplaces))    
			nplaces = ndigits+1;
		if(anum<0. )   
			nplaces +=1;
		if(nexp<0)   
			nplaces = nplaces+ndigits-1;
		if(nplacesmax<nplaces)   
			nplacesmax = nplaces;

		nformat = ndigits-(nexp+1);
		if(nformat<0)   nformat = 0;
	
		if (anumnorm>-azeronorm && anumnorm<azeronorm)
			sprintf(str,"%.4g",0.0);
		else	{
			sprintf(sformat,"%%.%df",nformat);
			sprintf(str,sformat,anumnorm);
		}

		if (style!=NORMAL) {
			moveto(numb,ya);
			rotate(90.0);
			justshow(-0.5,str);
			rotate(-90.0);
		}
		else   {
			moveto(numb,yas); 
			justshow(-1.,str);
		}
	}
	/* draw exponential mulitplier for axis2 */
	if(nexplot2!=0)  {
		if(style==NORMAL)   {
			numb2 = size2+.5*labelCH;
			moveto(labelb,numb2);
			show("x10");
			xb = labelb+1.5*labelCW;
			numb2 = numb2+0.5*labelCA;
			moveto(xb,numb2);
			sprintf(str2,"%d",nexplot2);
			show(str2);
		}
		else   {
			moveto(labelb,size2-.5*labelCW);
			rotate(90.);
			show("x10");
			rotate(-90.);
			moveto(labelb-.35*labelCH,size2+1.0*labelCW);
			sprintf(str2,"%d",nexplot2);
			rotate(90.);
			show(str2);
			rotate(-90.);
		}
	}
	stroke();

	/* draw axis2 tick marks */
	if(style==NORMAL)   labelb = numb-0.5*nplacesmax*labelCW;	
	dtic = dnum/ntic;
	for (atic=fnum-ntic*dtic-dtic; atic<=amax; atic+=dtic) {
		if (atic<amin) continue;
		ya = base+scale*atic;
		moveto(0.0,ya);  lineto(ticb/2,ya);
	}
	stroke();
	
	/* draw axis2 grid lines */
	if (grid2==SOLID) {
		grided = 1;
		ndash = 0;
	} else if (grid2==DASH) {
		grided = 1;
		ndash = 1;  dash[0] = 10;
	} else if (grid2==DOT) {
		grided = 1;
		ndash = 2;  dash[0] = 1;  dash[1] = 5;
	} else
		grided = 0;
	if (grided) {
		for (anum=fnum; anum<=amax; anum+=dnum) {
			if (anum<amin) continue;
			ya = base+scale*anum;
			moveto(0.0,ya);  lineto(size1,ya);
		}
		setdash(dash,ndash,0.0);
		stroke();
		setdash(dash,0,0.0);
	}
	
	/* draw axis2 label */
	moveto(labelb,size2/2.0);
	rotate(90.0);
	justshow(-0.5,label2);
	rotate(-90.0);

	/* draw title */
	setfont(titleFont,titleSize);
	if (style==NORMAL) {
		numb2 = size2+0.5*titleCH-titleCD;
		if(nexplot2!=0)	numb2 = numb2+.5*(labelCH+labelCA);
		moveto(size1/2.0,numb2);
		justshow(-0.5,title);
	} else {
		numb2 = size1+0.5*titleCH+titleCA;
		if(nexplot1!=0)	numb2 = numb2+labelCH;
		moveto(numb2,size2/2.0);
		rotate(90.0);
		justshow(-0.5,title);
		rotate(-90.0);
	}

	/* draw axes box */
	moveto(0.0,0.0);
	lineto(size1,0.0);
	lineto(size1,size2);
	lineto(0.0,size2);
	lineto(0.0,0.0);
	stroke();

	/* restore graphics state */
	grestore();
}
Esempio n. 3
0
bool areSticksInApModePosition(uint16_t ap_mode)
{
    return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
}
Esempio n. 4
0
 int ctbcon_(char *norm, char *uplo, char *diag, int *n, 
	int *kd, complex *ab, int *ldab, float *rcond, complex *work, 
	float *rwork, int *info)
{
    /* System generated locals */
    int ab_dim1, ab_offset, i__1;
    float r__1, r__2;

    /* Builtin functions */
    double r_imag(complex *);

    /* Local variables */
    int ix, kase, kase1;
    float scale;
    extern int lsame_(char *, char *);
    int isave[3];
    float anorm;
    int upper;
    extern  int clacn2_(int *, complex *, complex *, float 
	    *, int *, int *);
    float xnorm;
    extern int icamax_(int *, complex *, int *);
    extern double clantb_(char *, char *, char *, int *, int *, 
	    complex *, int *, float *), slamch_(
	    char *);
    extern  int clatbs_(char *, char *, char *, char *, 
	    int *, int *, complex *, int *, complex *, float *, 
	    float *, int *), xerbla_(char *
, int *);
    float ainvnm;
    extern  int csrscl_(int *, float *, complex *, int 
	    *);
    int onenrm;
    char normin[1];
    float smlnum;
    int nounit;


/*  -- LAPACK routine (version 3.2) -- */
/*     Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
/*     November 2006 */

/*     Modified to call CLACN2 in place of CLACON, 10 Feb 03, SJH. */

/*     .. Scalar Arguments .. */
/*     .. */
/*     .. Array Arguments .. */
/*     .. */

/*  Purpose */
/*  ======= */

/*  CTBCON estimates the reciprocal of the condition number of a */
/*  triangular band matrix A, in either the 1-norm or the infinity-norm. */

/*  The norm of A is computed and an estimate is obtained for */
/*  norm(inv(A)), then the reciprocal of the condition number is */
/*  computed as */
/*     RCOND = 1 / ( norm(A) * norm(inv(A)) ). */

/*  Arguments */
/*  ========= */

/*  NORM    (input) CHARACTER*1 */
/*          Specifies whether the 1-norm condition number or the */
/*          infinity-norm condition number is required: */
/*          = '1' or 'O':  1-norm; */
/*          = 'I':         Infinity-norm. */

/*  UPLO    (input) CHARACTER*1 */
/*          = 'U':  A is upper triangular; */
/*          = 'L':  A is lower triangular. */

/*  DIAG    (input) CHARACTER*1 */
/*          = 'N':  A is non-unit triangular; */
/*          = 'U':  A is unit triangular. */

/*  N       (input) INTEGER */
/*          The order of the matrix A.  N >= 0. */

/*  KD      (input) INTEGER */
/*          The number of superdiagonals or subdiagonals of the */
/*          triangular band matrix A.  KD >= 0. */

/*  AB      (input) COMPLEX array, dimension (LDAB,N) */
/*          The upper or lower triangular band matrix A, stored in the */
/*          first kd+1 rows of the array. The j-th column of A is stored */
/*          in the j-th column of the array AB as follows: */
/*          if UPLO = 'U', AB(kd+1+i-j,j) = A(i,j) for MAX(1,j-kd)<=i<=j; */
/*          if UPLO = 'L', AB(1+i-j,j)    = A(i,j) for j<=i<=MIN(n,j+kd). */
/*          If DIAG = 'U', the diagonal elements of A are not referenced */
/*          and are assumed to be 1. */

/*  LDAB    (input) INTEGER */
/*          The leading dimension of the array AB.  LDAB >= KD+1. */

/*  RCOND   (output) REAL */
/*          The reciprocal of the condition number of the matrix A, */
/*          computed as RCOND = 1/(norm(A) * norm(inv(A))). */

/*  WORK    (workspace) COMPLEX array, dimension (2*N) */

/*  RWORK   (workspace) REAL array, dimension (N) */

/*  INFO    (output) INTEGER */
/*          = 0:  successful exit */
/*          < 0:  if INFO = -i, the i-th argument had an illegal value */

/*  ===================================================================== */

/*     .. Parameters .. */
/*     .. */
/*     .. Local Scalars .. */
/*     .. */
/*     .. Local Arrays .. */
/*     .. */
/*     .. External Functions .. */
/*     .. */
/*     .. External Subroutines .. */
/*     .. */
/*     .. Intrinsic Functions .. */
/*     .. */
/*     .. Statement Functions .. */
/*     .. */
/*     .. Statement Function definitions .. */
/*     .. */
/*     .. Executable Statements .. */

/*     Test the input parameters. */

    /* Parameter adjustments */
    ab_dim1 = *ldab;
    ab_offset = 1 + ab_dim1;
    ab -= ab_offset;
    --work;
    --rwork;

    /* Function Body */
    *info = 0;
    upper = lsame_(uplo, "U");
    onenrm = *(unsigned char *)norm == '1' || lsame_(norm, "O");
    nounit = lsame_(diag, "N");

    if (! onenrm && ! lsame_(norm, "I")) {
	*info = -1;
    } else if (! upper && ! lsame_(uplo, "L")) {
	*info = -2;
    } else if (! nounit && ! lsame_(diag, "U")) {
	*info = -3;
    } else if (*n < 0) {
	*info = -4;
    } else if (*kd < 0) {
	*info = -5;
    } else if (*ldab < *kd + 1) {
	*info = -7;
    }
    if (*info != 0) {
	i__1 = -(*info);
	xerbla_("CTBCON", &i__1);
	return 0;
    }

/*     Quick return if possible */

    if (*n == 0) {
	*rcond = 1.f;
	return 0;
    }

    *rcond = 0.f;
    smlnum = slamch_("Safe minimum") * (float) MAX(*n,1);

/*     Compute the 1-norm of the triangular matrix A or A'. */

    anorm = clantb_(norm, uplo, diag, n, kd, &ab[ab_offset], ldab, &rwork[1]);

/*     Continue only if ANORM > 0. */

    if (anorm > 0.f) {

/*        Estimate the 1-norm of the inverse of A. */

	ainvnm = 0.f;
	*(unsigned char *)normin = 'N';
	if (onenrm) {
	    kase1 = 1;
	} else {
	    kase1 = 2;
	}
	kase = 0;
L10:
	clacn2_(n, &work[*n + 1], &work[1], &ainvnm, &kase, isave);
	if (kase != 0) {
	    if (kase == kase1) {

/*              Multiply by inv(A). */

		clatbs_(uplo, "No transpose", diag, normin, n, kd, &ab[
			ab_offset], ldab, &work[1], &scale, &rwork[1], info);
	    } else {

/*              Multiply by inv(A'). */

		clatbs_(uplo, "Conjugate transpose", diag, normin, n, kd, &ab[
			ab_offset], ldab, &work[1], &scale, &rwork[1], info);
	    }
	    *(unsigned char *)normin = 'Y';

/*           Multiply by 1/SCALE if doing so will not cause overflow. */

	    if (scale != 1.f) {
		ix = icamax_(n, &work[1], &c__1);
		i__1 = ix;
		xnorm = (r__1 = work[i__1].r, ABS(r__1)) + (r__2 = r_imag(&
			work[ix]), ABS(r__2));
		if (scale < xnorm * smlnum || scale == 0.f) {
		    goto L20;
		}
		csrscl_(n, &scale, &work[1], &c__1);
	    }
	    goto L10;
	}

/*        Compute the estimate of the reciprocal condition number. */

	if (ainvnm != 0.f) {
	    *rcond = 1.f / anorm / ainvnm;
	}
    }

L20:
    return 0;

/*     End of CTBCON */

} /* ctbcon_ */
Esempio n. 5
0
int main (int argc, char **argv)
{
    double resid [4], t, ta, tf, ts [3], tot, bnorm, xnorm, anorm, rnorm, fl,
        anz, axbnorm, rnorm2, resid2, rcond ;
    FILE *f ;
    cholmod_sparse *A ;
    cholmod_dense *X = NULL, *B, *W, *R = NULL ;
    double one [2], zero [2], minusone [2], beta [2], xlnz ;
    cholmod_common Common, *cm ;
    cholmod_factor *L ;
    double *Bx, *Rx, *Xx ;
    int i, n, isize, xsize, ordering, xtype, s, ss, lnz ;
    int trial, method, L_is_super ;
    int ver [3] ;

    ts[0] = 0.;
    ts[1] = 0.;
    ts[2] = 0.;

    /* ---------------------------------------------------------------------- */
    /* get the file containing the input matrix */
    /* ---------------------------------------------------------------------- */

    ff = NULL ;
    if (argc > 1)
    {
	if ((f = fopen (argv [1], "r")) == NULL)
	{
	    my_handler (CHOLMOD_INVALID, __FILE__, __LINE__,
		    "unable to open file") ;
	}
	ff = f ;
    }
    else
    {
	f = stdin ;
    }

    /* ---------------------------------------------------------------------- */
    /* start CHOLMOD and set parameters */
    /* ---------------------------------------------------------------------- */

    cm = &Common ;
    cholmod_start (cm) ;
    CHOLMOD_FUNCTION_DEFAULTS ;     /* just for testing (not required) */

    /* use default parameter settings, except for the error handler.  This
     * demo program terminates if an error occurs (out of memory, not positive
     * definite, ...).  It makes the demo program simpler (no need to check
     * CHOLMOD error conditions).  This non-default parameter setting has no
     * effect on performance. */
    cm->error_handler = my_handler ;

    /* Note that CHOLMOD will do a supernodal LL' or a simplicial LDL' by
     * default, automatically selecting the latter if flop/nnz(L) < 40. */

    /* ---------------------------------------------------------------------- */
    /* create basic scalars */
    /* ---------------------------------------------------------------------- */

    zero [0] = 0 ;
    zero [1] = 0 ;
    one [0] = 1 ;
    one [1] = 0 ;
    minusone [0] = -1 ;
    minusone [1] = 0 ;
    beta [0] = 1e-6 ;
    beta [1] = 0 ;

    /* ---------------------------------------------------------------------- */
    /* read in a matrix */
    /* ---------------------------------------------------------------------- */

    printf ("\n---------------------------------- cholmod_demo:\n") ;
    cholmod_version (ver) ;
    printf ("cholmod version %d.%d.%d\n", ver [0], ver [1], ver [2]) ;
    SuiteSparse_version (ver) ;
    printf ("SuiteSparse version %d.%d.%d\n", ver [0], ver [1], ver [2]) ;
    A = cholmod_read_sparse (f, cm) ;
    if (ff != NULL)
    {
        fclose (ff) ;
        ff = NULL ;
    }
    xtype = A->xtype ;
    anorm = 1 ;
#ifndef NMATRIXOPS
    anorm = cholmod_norm_sparse (A, 0, cm) ;
    printf ("norm (A,inf) = %g\n", anorm) ;
    printf ("norm (A,1)   = %g\n", cholmod_norm_sparse (A, 1, cm)) ;
#endif
    cholmod_print_sparse (A, "A", cm) ;

    if (A->nrow > A->ncol)
    {
	/* Transpose A so that A'A+beta*I will be factorized instead */
	cholmod_sparse *C = cholmod_transpose (A, 2, cm) ;
	cholmod_free_sparse (&A, cm) ;
	A = C ;
	printf ("transposing input matrix\n") ;
    }

    /* ---------------------------------------------------------------------- */
    /* create an arbitrary right-hand-side */
    /* ---------------------------------------------------------------------- */

    n = A->nrow ;
    B = cholmod_zeros (n, 1, xtype, cm) ;
    Bx = B->x ;

#if GHS
    {
	/* b = A*ones(n,1), used by Gould, Hu, and Scott in their experiments */
	cholmod_dense *X0 ;
	X0 = cholmod_ones (A->ncol, 1, xtype, cm) ;
	cholmod_sdmult (A, 0, one, zero, X0, B, cm) ;
	cholmod_free_dense (&X0, cm) ;
    }
#else
    if (xtype == CHOLMOD_REAL)
    {
	/* real case */
	for (i = 0 ; i < n ; i++)
	{
	    double x = n ;
	    Bx [i] = 1 + i / x ;
	}
    }
    else
    {
	/* complex case */
	for (i = 0 ; i < n ; i++)
	{
	    double x = n ;
	    Bx [2*i  ] = 1 + i / x ;		/* real part of B(i) */
	    Bx [2*i+1] = (x/2 - i) / (3*x) ;	/* imag part of B(i) */
	}
    }
#endif

    cholmod_print_dense (B, "B", cm) ;
    bnorm = 1 ;
#ifndef NMATRIXOPS
    bnorm = cholmod_norm_dense (B, 0, cm) ;	/* max norm */
    printf ("bnorm %g\n", bnorm) ;
#endif

    /* ---------------------------------------------------------------------- */
    /* analyze and factorize */
    /* ---------------------------------------------------------------------- */

    t = CPUTIME ;
    L = cholmod_analyze (A, cm) ;
    ta = CPUTIME - t ;
    ta = MAX (ta, 0) ;

    printf ("Analyze: flop %g lnz %g\n", cm->fl, cm->lnz) ;

    if (A->stype == 0)
    {
	printf ("Factorizing A*A'+beta*I\n") ;
	t = CPUTIME ;
	cholmod_factorize_p (A, beta, NULL, 0, L, cm) ;
	tf = CPUTIME - t ;
	tf = MAX (tf, 0) ;
    }
    else
    {
	printf ("Factorizing A\n") ;
	t = CPUTIME ;
	cholmod_factorize (A, L, cm) ;
	tf = CPUTIME - t ;
	tf = MAX (tf, 0) ;
    }

    cholmod_print_factor (L, "L", cm) ;

    /* determine the # of integers's and reals's in L.  See cholmod_free */
    if (L->is_super)
    {
	s = L->nsuper + 1 ;
	xsize = L->xsize ;
	ss = L->ssize ;
	isize =
	    n	/* L->Perm */
	    + n	/* L->ColCount, nz in each column of 'pure' L */
	    + s	/* L->pi, column pointers for L->s */
	    + s	/* L->px, column pointers for L->x */
	    + s	/* L->super, starting column index of each supernode */
	    + ss ;	/* L->s, the pattern of the supernodes */
    }
    else
    {
	/* this space can increase if you change parameters to their non-
	 * default values (cm->final_pack, for example). */
	lnz = L->nzmax ;
	xsize = lnz ;
	isize =
	    n	/* L->Perm */
	    + n	/* L->ColCount, nz in each column of 'pure' L */
	    + n+1	/* L->p, column pointers */
	    + lnz	/* L->i, integer row indices */
	    + n	/* L->nz, nz in each column of L */
	    + n+2	/* L->next, link list */
	    + n+2 ;	/* L->prev, link list */
    }

    /* solve with Bset will change L from simplicial to supernodal */
    rcond = cholmod_rcond (L, cm) ;
    L_is_super = L->is_super ;

    /* ---------------------------------------------------------------------- */
    /* solve */
    /* ---------------------------------------------------------------------- */

    for (method = 0 ; method <= 3 ; method++)
    {
        double x = n ;
        resid [method] = -1 ;       /* not yet computed */

        if (method == 0)
        {
            /* basic solve, just once */
            t = CPUTIME ;
            X = cholmod_solve (CHOLMOD_A, L, B, cm) ;
            ts [0] = CPUTIME - t ;
            ts [0] = MAX (ts [0], 0) ;
        }
        else if (method == 1)
        {
            /* basic solve, many times, but keep the last one */
            t = CPUTIME ;
            for (trial = 0 ; trial < NTRIALS ; trial++)
            {
                cholmod_free_dense (&X, cm) ;
                Bx [0] = 1 + trial / x ;        /* tweak B each iteration */
                X = cholmod_solve (CHOLMOD_A, L, B, cm) ;
            }
            ts [1] = CPUTIME - t ;
            ts [1] = MAX (ts [1], 0) / NTRIALS ;
        }
        else if (method == 2)
        {
            /* solve with reused workspace */
            cholmod_dense *Ywork = NULL, *Ework = NULL ;
            cholmod_free_dense (&X, cm) ;

            t = CPUTIME ;
            for (trial = 0 ; trial < NTRIALS ; trial++)
            {
                Bx [0] = 1 + trial / x ;        /* tweak B each iteration */
                cholmod_solve2 (CHOLMOD_A, L, B, NULL, &X, NULL,
                    &Ywork, &Ework, cm) ;
            }
            cholmod_free_dense (&Ywork, cm) ;
            cholmod_free_dense (&Ework, cm) ;
            ts [2] = CPUTIME - t ;
            ts [2] = MAX (ts [2], 0) / NTRIALS ;
        }
        else
        {
            /* solve with reused workspace and sparse Bset */
            cholmod_dense *Ywork = NULL, *Ework = NULL ;
            cholmod_dense *X2 = NULL, *B2 = NULL ;
            cholmod_sparse *Bset, *Xset = NULL ;
            int *Bsetp, *Bseti, *Xsetp, *Xseti, xlen, j, k, *Lnz ;
            double *X1x, *X2x, *B2x, err ;
            FILE *timelog = fopen ("timelog.m", "w") ;
            if (timelog) fprintf (timelog, "results = [\n") ;

            B2 = cholmod_zeros (n, 1, xtype, cm) ;
            B2x = B2->x ;

            Bset = cholmod_allocate_sparse (n, 1, 1, FALSE, TRUE, 0,
                CHOLMOD_PATTERN, cm) ;
            Bsetp = Bset->p ;
            Bseti = Bset->i ;
            Bsetp [0] = 0 ;     /* nnz(B) is 1 (it can be anything) */
            Bsetp [1] = 1 ;
            resid [3] = 0 ;

            for (i = 0 ; i < MIN (100,n) ; i++)
            {
                /* B (i) is nonzero, all other entries are ignored
                   (implied to be zero) */
                Bseti [0] = i ;
                if (xtype == CHOLMOD_REAL)
                {
                    B2x [i] = 3.1 * i + 0.9 ;
                }
                else
                {
                    B2x [2*i  ] = i + 0.042 ;
                    B2x [2*i+1] = i - 92.7 ;
                }

                /* first get the entire solution, to compare against */
                cholmod_solve2 (CHOLMOD_A, L, B2, NULL, &X, NULL,
                    &Ywork, &Ework, cm) ;

                /* now get the sparse solutions; this will change L from
                   supernodal to simplicial */

                if (i == 0)
                {
                    /* first solve can be slower because it has to allocate
                       space for X2, Xset, etc, and change L.
                       So don't time it */
                    cholmod_solve2 (CHOLMOD_A, L, B2, Bset, &X2, &Xset,
                        &Ywork, &Ework, cm) ;
                }

                t = CPUTIME ;
                for (trial = 0 ; trial < NTRIALS ; trial++)
                {
                    /* solve Ax=b but only to get x(i).
                       b is all zero except for b(i).
                       This takes O(xlen) time */
                    cholmod_solve2 (CHOLMOD_A, L, B2, Bset, &X2, &Xset,
                        &Ywork, &Ework, cm) ;
                }
                t = CPUTIME - t ;
                t = MAX (t, 0) / NTRIALS ;

                /* check the solution and log the time */
                Xsetp = Xset->p ;
                Xseti = Xset->i ;
                xlen = Xsetp [1] ;
                X1x = X->x ;
                X2x = X2->x ;
                Lnz = L->nz ;

                /*
                printf ("\ni %d xlen %d  (%p %p)\n", i, xlen, X1x, X2x) ;
                */

                if (xtype == CHOLMOD_REAL)
                {
                    fl = 2 * xlen ;
                    for (k = 0 ; k < xlen ; k++)
                    {
                        j = Xseti [k] ;
                        fl += 4 * Lnz [j] ;
                        err = X1x [j] - X2x [j] ;
                        err = ABS (err) ;
                        resid [3] = MAX (resid [3], err) ;
                    }
                }
                else
                {
                    fl = 16 * xlen ;
                    for (k = 0 ; k < xlen ; k++)
                    {
                        j = Xseti [k] ;
                        fl += 16 * Lnz [j] ;
                        err = X1x [2*j  ] - X2x [2*j  ] ;
                        err = ABS (err) ;
                        resid [3] = MAX (resid [3], err) ;
                        err = X1x [2*j+1] - X2x [2*j+1] ;
                        err = ABS (err) ;
                        resid [3] = MAX (resid [3], err) ;
                    }
                }
                if (timelog) fprintf (timelog, "%g %g %g %g\n",
                    (double) i, (double) xlen, fl, t);

                /* clear B for the next test */
                if (xtype == CHOLMOD_REAL)
                {
                    B2x [i] = 0 ;
                }
                else
                {
                    B2x [2*i  ] = 0 ;
                    B2x [2*i+1] = 0 ;
                }

            }

            if (timelog)
            {
                fprintf (timelog, "] ; resid = %g ;\n", resid [3]) ;
                fprintf (timelog, "lnz = %g ;\n", cm->lnz) ;
                fprintf (timelog, "t = %g ;   %% dense solve time\n", ts [2]) ;
                fclose (timelog) ;
            }

#ifndef NMATRIXOPS
            resid [3] = resid [3] / cholmod_norm_dense (X, 1, cm) ;
#endif

            cholmod_free_dense (&Ywork, cm) ;
            cholmod_free_dense (&Ework, cm) ;
            cholmod_free_dense (&X2, cm) ;
            cholmod_free_dense (&B2, cm) ;
            cholmod_free_sparse (&Xset, cm) ;
            cholmod_free_sparse (&Bset, cm) ;
        }

        /* ------------------------------------------------------------------ */
        /* compute the residual */
        /* ------------------------------------------------------------------ */

        if (method < 3)
        {
#ifndef NMATRIXOPS

            if (A->stype == 0)
            {
                /* (AA'+beta*I)x=b is the linear system that was solved */
                /* W = A'*X */
                W = cholmod_allocate_dense (A->ncol, 1, A->ncol, xtype, cm) ;
                cholmod_sdmult (A, 2, one, zero, X, W, cm) ;
                /* R = B - beta*X */
                cholmod_free_dense (&R, cm) ;
                R = cholmod_zeros (n, 1, xtype, cm) ;
                Rx = R->x ;
                Xx = X->x ;
                if (xtype == CHOLMOD_REAL)
                {
                    for (i = 0 ; i < n ; i++)
                    {
                        Rx [i] = Bx [i] - beta [0] * Xx [i] ;
                    }
                }
                else
                {
                    /* complex case */
                    for (i = 0 ; i < n ; i++)
                    {
                        Rx [2*i  ] = Bx [2*i  ] - beta [0] * Xx [2*i  ] ;
                        Rx [2*i+1] = Bx [2*i+1] - beta [0] * Xx [2*i+1] ;
                    }
                }
                /* R = A*W - R */
                cholmod_sdmult (A, 0, one, minusone, W, R, cm) ;
                cholmod_free_dense (&W, cm) ;
            }
            else
            {
                /* Ax=b was factorized and solved, R = B-A*X */
                cholmod_free_dense (&R, cm) ;
                R = cholmod_copy_dense (B, cm) ;
                cholmod_sdmult (A, 0, minusone, one, X, R, cm) ;
            }
            rnorm = -1 ;
            xnorm = 1 ;
            rnorm = cholmod_norm_dense (R, 0, cm) ;	    /* max abs. entry */
            xnorm = cholmod_norm_dense (X, 0, cm) ;	    /* max abs. entry */
            axbnorm = (anorm * xnorm + bnorm + ((n == 0) ? 1 : 0)) ;
            resid [method] = rnorm / axbnorm ;
#else
            printf ("residual not computed (requires CHOLMOD/MatrixOps)\n") ;
#endif
        }
    }

    tot = ta + tf + ts [0] ;

    /* ---------------------------------------------------------------------- */
    /* iterative refinement (real symmetric case only) */
    /* ---------------------------------------------------------------------- */

    resid2 = -1 ;
#ifndef NMATRIXOPS
    if (A->stype != 0 && A->xtype == CHOLMOD_REAL)
    {
	cholmod_dense *R2 ;

	/* R2 = A\(B-A*X) */
	R2 = cholmod_solve (CHOLMOD_A, L, R, cm) ;
	/* compute X = X + A\(B-A*X) */
	Xx = X->x ;
	Rx = R2->x ;
	for (i = 0 ; i < n ; i++)
	{
	    Xx [i] = Xx [i] + Rx [i] ;
	}
	cholmod_free_dense (&R2, cm) ;
	cholmod_free_dense (&R, cm) ;

	/* compute the new residual, R = B-A*X */
        cholmod_free_dense (&R, cm) ;
	R = cholmod_copy_dense (B, cm) ;
	cholmod_sdmult (A, 0, minusone, one, X, R, cm) ;
	rnorm2 = cholmod_norm_dense (R, 0, cm) ;
	resid2 = rnorm2 / axbnorm ;
    }
#endif

    cholmod_free_dense (&R, cm) ;

    /* ---------------------------------------------------------------------- */
    /* print results */
    /* ---------------------------------------------------------------------- */

    anz = cm->anz ;
    for (i = 0 ; i < CHOLMOD_MAXMETHODS ; i++)
    {
	fl = cm->method [i].fl ;
	xlnz = cm->method [i].lnz ;
	cm->method [i].fl = -1 ;
	cm->method [i].lnz = -1 ;
	ordering = cm->method [i].ordering ;
	if (fl >= 0)
	{
	    printf ("Ordering: ") ;
	    if (ordering == CHOLMOD_POSTORDERED) printf ("postordered ") ;
	    if (ordering == CHOLMOD_NATURAL)     printf ("natural ") ;
	    if (ordering == CHOLMOD_GIVEN)	     printf ("user    ") ;
	    if (ordering == CHOLMOD_AMD)	     printf ("AMD     ") ;
	    if (ordering == CHOLMOD_METIS)	     printf ("METIS   ") ;
	    if (ordering == CHOLMOD_NESDIS)      printf ("NESDIS  ") ;
	    if (xlnz > 0)
	    {
		printf ("fl/lnz %10.1f", fl / xlnz) ;
	    }
	    if (anz > 0)
	    {
		printf ("  lnz/anz %10.1f", xlnz / anz) ;
	    }
	    printf ("\n") ;
	}
    }

    printf ("ints in L: %15.0f, doubles in L: %15.0f\n",
        (double) isize, (double) xsize) ;
    printf ("factor flops %g nnz(L) %15.0f (w/no amalgamation)\n",
	    cm->fl, cm->lnz) ;
    if (A->stype == 0)
    {
	printf ("nnz(A):    %15.0f\n", cm->anz) ;
    }
    else
    {
	printf ("nnz(A*A'): %15.0f\n", cm->anz) ;
    }
    if (cm->lnz > 0)
    {
	printf ("flops / nnz(L):  %8.1f\n", cm->fl / cm->lnz) ;
    }
    if (anz > 0)
    {
	printf ("nnz(L) / nnz(A): %8.1f\n", cm->lnz / cm->anz) ;
    }
    printf ("analyze cputime:  %12.4f\n", ta) ;
    printf ("factor  cputime:   %12.4f mflop: %8.1f\n", tf,
	(tf == 0) ? 0 : (1e-6*cm->fl / tf)) ;
    printf ("solve   cputime:   %12.4f mflop: %8.1f\n", ts [0],
	(ts [0] == 0) ? 0 : (1e-6*4*cm->lnz / ts [0])) ;
    printf ("overall cputime:   %12.4f mflop: %8.1f\n", 
	    tot, (tot == 0) ? 0 : (1e-6 * (cm->fl + 4 * cm->lnz) / tot)) ;
    printf ("solve   cputime:   %12.4f mflop: %8.1f (%d trials)\n", ts [1],
	(ts [1] == 0) ? 0 : (1e-6*4*cm->lnz / ts [1]), NTRIALS) ;
    printf ("solve2  cputime:   %12.4f mflop: %8.1f (%d trials)\n", ts [2],
	(ts [2] == 0) ? 0 : (1e-6*4*cm->lnz / ts [2]), NTRIALS) ;
    printf ("peak memory usage: %12.0f (MB)\n",
	    (double) (cm->memory_usage) / 1048576.) ;
    printf ("residual (|Ax-b|/(|A||x|+|b|)): ") ;
    for (method = 0 ; method <= 3 ; method++)
    {
        printf ("%8.2e ", resid [method]) ;
    }
    printf ("\n") ;
    if (resid2 >= 0)
    {
	printf ("residual %8.1e (|Ax-b|/(|A||x|+|b|))"
		" after iterative refinement\n", resid2) ;
    }

    printf ("rcond    %8.1e\n\n", rcond) ;

    if (L_is_super)
    {
        cholmod_gpu_stats (cm) ;
    }

    cholmod_free_factor (&L, cm) ;
    cholmod_free_dense (&X, cm) ;

    /* ---------------------------------------------------------------------- */
    /* free matrices and finish CHOLMOD */
    /* ---------------------------------------------------------------------- */

    cholmod_free_sparse (&A, cm) ;
    cholmod_free_dense (&B, cm) ;
    cholmod_finish (cm) ;
    
    return (0) ;
}
Esempio n. 6
0
void csplin(size_t n, float x[], float y[], float yd[][4])
/*****************************************************************************
compute cubic spline interpolation coefficients for interpolation with
continuous second derivatives
******************************************************************************
Input:
n		number of samples
x  		array[n] of monotonically increasing or decreasing abscissae
y		array[n] of ordinates

Output:
yd		array[n][4] of cubic interpolation coefficients (see notes)
******************************************************************************
Notes:
The computed cubic spline coefficients are as follows:
yd[i][0] = y(x[i])    (the value of y at x = x[i])
yd[i][1] = y'(x[i])   (the 1st derivative of y at x = x[i])
yd[i][2] = y''(x[i])  (the 2nd derivative of y at x = x[i])
yd[i][3] = y'''(x[i]) (the 3rd derivative of y at x = x[i])

To evaluate y(x) for x between x[i] and x[i+1] and h = x-x[i],
use the computed coefficients as follows:
y(x) = yd[i][0]+h*(yd[i][1]+h*(yd[i][2]/2.0+h*yd[i][3]/6.0))
******************************************************************************
Author:  Dave Hale, Colorado School of Mines, 10/03/89
Modified:  Dave Hale, Colorado School of Mines, 02/28/91
    changed to work for n=1.
Modified:  Dave Hale, Colorado School of Mines, 08/04/91
    fixed bug in computation of left end derivative
*****************************************************************************/
{
	size_t i;
	float h1, h2, del1, del2, dmax, hsum, w1, w2, divdf3, sleft, sright, alpha, t;

	/* if n=1, then use constant interpolation */
	if (n == 1) {
		yd[0][0] = y[0];
		yd[0][1] = 0.0;
		yd[0][2] = 0.0;
		yd[0][3] = 0.0;
		return;

		/* else, if n=2, then use linear interpolation */
	} else if (n == 2) {
		yd[0][0] = y[0];
		yd[1][0] = y[1];
		yd[0][1] = yd[1][1] = (y[1] - y[0]) / (x[1] - x[0]);
		yd[0][2] = yd[1][2] = 0.0;
		yd[0][3] = yd[1][3] = 0.0;
		return;
	}

	/* set left end derivative via shape-preserving 3-point formula */
	h1    = x[1] - x[0];
	h2    = x[2] - x[1];
	hsum  = h1 + h2;
	del1  = (y[1] - y[0]) / h1;
	del2  = (y[2] - y[1]) / h2;
	w1    = (h1 + hsum) / hsum;
	w2    = -h1 / hsum;
	sleft = w1 * del1 + w2 * del2;
	if (sleft * del1 <= 0.0)
		sleft = 0.0;
	else if (del1 * del2 < 0.0) {
		dmax                              = 3.0 * del1;
		if (ABS(sleft) > ABS(dmax)) sleft = dmax;
	}

	/* set right end derivative via shape-preserving 3-point formula */
	h1     = x[n - 2] - x[n - 3];
	h2     = x[n - 1] - x[n - 2];
	hsum   = h1 + h2;
	del1   = (y[n - 2] - y[n - 3]) / h1;
	del2   = (y[n - 1] - y[n - 2]) / h2;
	w1     = -h2 / hsum;
	w2     = (h2 + hsum) / hsum;
	sright = w1 * del1 + w2 * del2;
	if (sright * del2 <= 0.0)
		sright = 0.0;
	else if (del1 * del2 < 0.0) {
		dmax                                = 3.0 * del2;
		if (ABS(sright) > ABS(dmax)) sright = dmax;
	}

	/* compute tridiagonal system coefficients and right-hand-side */
	yd[0][0] = 1.0;
	yd[0][2] = 2.0 * sleft;
	for (i = 1; i < n - 1; i++) {
		h1       = x[i] - x[i - 1];
		h2       = x[i + 1] - x[i];
		del1     = (y[i] - y[i - 1]) / h1;
		del2     = (y[i + 1] - y[i]) / h2;
		alpha    = h2 / (h1 + h2);
		yd[i][0] = alpha;
		yd[i][2] = 3.0 * (alpha * del1 + (1.0 - alpha) * del2);
	}
	yd[n - 1][0] = 0.0;
	yd[n - 1][2] = 2.0 * sright;

	/* solve tridiagonal system for slopes */
	t        = 2.0;
	yd[0][1] = yd[0][2] / t;
	for (i = 1; i < n; i++) {
		yd[i][3] = (1.0 - yd[i - 1][0]) / t;
		t        = 2.0 - yd[i][0] * yd[i][3];
		yd[i][1] = (yd[i][2] - yd[i][0] * yd[i - 1][1]) / t;
	}
	for (i = n - 2; ; i--) {
		yd[i][1] -= yd[i + 1][3] * yd[i + 1][1];
		
		//i is size_t so >= 0 above doesn't work
		if (i == 0)
			break;
	}

	/* copy ordinates into output array */
	for (i = 0; i < n; i++) yd[i][0] = y[i];

	/* compute 2nd and 3rd derivatives of cubic polynomials */
	for (i = 0; i < n - 1; i++) {
		h2       = x[i + 1] - x[i];
		del2     = (y[i + 1] - y[i]) / h2;
		divdf3   = yd[i][1] + yd[i + 1][1] - 2.0 * del2;
		yd[i][2] = 2.0 * (del2 - yd[i][1] - divdf3) / h2;
		yd[i][3] = (divdf3 / h2) * (6.0 / h2);
	}
	yd[n - 1][2] = yd[n - 2][2] + (x[n - 1] - x[n - 2]) * yd[n - 2][3];
	yd[n - 1][3] = yd[n - 2][3];
}
Esempio n. 7
0
CPLErr BTDataset::SetProjection( const char *pszNewProjection )

{
    CPLErr eErr = CE_None;

    CPLFree( pszProjection );
    pszProjection = CPLStrdup( pszNewProjection );

    bHeaderModified = TRUE;

/* -------------------------------------------------------------------- */
/*      Parse projection.                                               */
/* -------------------------------------------------------------------- */
    OGRSpatialReference oSRS( pszProjection );
    GInt16  nShortTemp;

/* -------------------------------------------------------------------- */
/*      Linear units.                                                   */
/* -------------------------------------------------------------------- */
    if( oSRS.IsGeographic() )
        nShortTemp = 0;
    else 
    {
        double dfLinear = oSRS.GetLinearUnits();

        if( ABS(dfLinear - 0.3048) < 0.0000001 )
            nShortTemp = 2;
        else if( ABS(dfLinear - atof(SRS_UL_US_FOOT_CONV)) < 0.00000001 )
            nShortTemp = 3;
        else
            nShortTemp = 1;
    }

    nShortTemp = CPL_LSBWORD16( 1 );
    memcpy( abyHeader + 22, &nShortTemp, 2 );
    
/* -------------------------------------------------------------------- */
/*      UTM Zone                                                        */
/* -------------------------------------------------------------------- */
    int bNorth;

    nShortTemp = (GInt16) oSRS.GetUTMZone( &bNorth );
    if( bNorth )
        nShortTemp = -nShortTemp;

    nShortTemp = CPL_LSBWORD16( nShortTemp );
    memcpy( abyHeader + 24, &nShortTemp, 2 );
    
/* -------------------------------------------------------------------- */
/*      Datum                                                           */
/* -------------------------------------------------------------------- */
    if( oSRS.GetAuthorityName( "GEOGCS|DATUM" ) != NULL
        && EQUAL(oSRS.GetAuthorityName( "GEOGCS|DATUM" ),"EPSG") )
        nShortTemp = (GInt16) (atoi(oSRS.GetAuthorityCode( "GEOGCS|DATUM" )) + 2000);
    else
        nShortTemp = -2;
    nShortTemp = CPL_LSBWORD16( nShortTemp ); /* datum unknown */
    memcpy( abyHeader + 26, &nShortTemp, 2 );

/* -------------------------------------------------------------------- */
/*      Write out the projection to a .prj file.                        */
/* -------------------------------------------------------------------- */
    const char  *pszPrjFile = CPLResetExtension( GetDescription(), "prj" );
    VSILFILE * fp;

    fp = VSIFOpenL( pszPrjFile, "wt" );
    if( fp != NULL )
    {
        VSIFPrintfL( fp, "%s\n", pszProjection );
        VSIFCloseL( fp );
        abyHeader[60] = 1;
    }
    else
    {
        CPLError( CE_Failure, CPLE_AppDefined, 
                  "Unable to write out .prj file." );
        eErr = CE_Failure;
    }

    return eErr;
}
Esempio n. 8
0
int
mpfr_set_f (mpfr_ptr y, mpf_srcptr x, mpfr_rnd_t rnd_mode)
{
  mp_limb_t *my, *mx, *tmp;
  unsigned long cnt, sx, sy;
  int inexact, carry = 0;
  MPFR_TMP_DECL(marker);

  sx = ABS(SIZ(x)); /* number of limbs of the mantissa of x */

  if (sx == 0) /* x is zero */
    {
      MPFR_SET_ZERO(y);
      MPFR_SET_POS(y);
      return 0; /* 0 is exact */
    }

  if (SIZ(x) * MPFR_FROM_SIGN_TO_INT(MPFR_SIGN(y)) < 0)
    MPFR_CHANGE_SIGN (y);

  sy = MPFR_LIMB_SIZE (y);
  my = MPFR_MANT(y);
  mx = PTR(x);

  count_leading_zeros(cnt, mx[sx - 1]);

  if (sy <= sx) /* we may have to round even when sy = sx */
    {
      unsigned long xprec = sx * GMP_NUMB_BITS;

      MPFR_TMP_MARK(marker);
      tmp = MPFR_TMP_LIMBS_ALLOC (sx);
      if (cnt)
        mpn_lshift (tmp, mx, sx, cnt);
      else
        /* FIXME: we may avoid the copy here, and directly call mpfr_round_raw
           on mx instead of tmp */
        MPN_COPY (tmp, mx, sx);
      carry = mpfr_round_raw (my, tmp, xprec, (SIZ(x) < 0), MPFR_PREC(y),
                              rnd_mode, &inexact);
      if (MPFR_UNLIKELY(carry)) /* result is a power of two */
        my[sy - 1] = MPFR_LIMB_HIGHBIT;
      MPFR_TMP_FREE(marker);
    }
  else
    {
      if (cnt)
        mpn_lshift (my + sy - sx, mx, sx, cnt);
      else
        MPN_COPY (my + sy - sx, mx, sx);
      MPN_ZERO(my, sy - sx);
      /* no rounding necessary, since y has a larger mantissa */
      inexact = 0;
    }

  /* warning: EXP(x) * GMP_NUMB_BITS may exceed the maximal exponent */
  if (EXP(x) > 1 + (__gmpfr_emax - 1) / GMP_NUMB_BITS)
    {
      /* EXP(x) >= 2 + floor((__gmpfr_emax-1)/GMP_NUMB_BITS)
         EXP(x) >= 2 + (__gmpfr_emax - GMP_NUMB_BITS) / GMP_NUMB_BITS
                >= 1 + __gmpfr_emax / GMP_NUMB_BITS
         EXP(x) * GMP_NUMB_BITS >= __gmpfr_emax + GMP_NUMB_BITS
         Since 0 <= cnt <= GMP_NUMB_BITS-1, and 0 <= carry <= 1,
         we have then EXP(x) * GMP_NUMB_BITS - cnt + carry > __gmpfr_emax */
      return mpfr_overflow (y, rnd_mode, MPFR_SIGN (y));
    }
  else
    {
      /* Do not use MPFR_SET_EXP as the exponent may be out of range. */
      MPFR_EXP (y) = EXP (x) * GMP_NUMB_BITS - (mpfr_exp_t) cnt + carry;
    }

  return mpfr_check_range (y, inexact, rnd_mode);
}
Esempio n. 9
0
/*------------------------------------------------------------------*/
static void
fill_kugel(struct state *st, int i, Pixmap buf, int setcol)
{
  double ra;
  int m,col,inc=1,inr=3,d;
  d=(int)((ABS(st->kugeln[i].r1)*2));
  if (d==0) d=1;
  
#ifdef FASTDRAW
  if(st->fastdraw && d<st->fastch)
    {
#	ifdef FASTCOPY
      XCopyArea(st->dpy, st->fastmask, buf, st->andgc, sum1ton(d)-(d+1)/2, 1,d,d,
		(int)(st->kugeln[i].x1)-d/2, (int)(st->kugeln[i].y1)-d/2);
      XCopyArea(st->dpy, st->fastcircles, buf, st->orgc, sum1ton(d)-(d+1)/2, 1,d,d,
		(int)(st->kugeln[i].x1)-d/2, (int)(st->kugeln[i].y1)-d/2);
#	else
      XPutImage(st->dpy, buf, st->andgc, st->fastmask[d-1], 0, 0,
		(int)(st->kugeln[i].x1)-d/2, (int)(st->kugeln[i].y1)-d/2, d, d);
      XPutImage(st->dpy, buf, st->orgc, st->fastcircles[d-1], 0, 0,
		(int)(st->kugeln[i].x1)-d/2, (int)(st->kugeln[i].y1)-d/2, d, d);
#	endif
    }
  else
#endif
    {
      if(ABS(st->kugeln[i].r1)<6.0) inr=9;
      
      for (m=0;m<=28;m+=inr)
	{
	  ra=st->kugeln[i].r1*sqrt(1-m*m/(28.0*28.0));
#ifdef PRTDBX
	  printf("Radius: %f\n",ra);
#endif
	  if(-ra< 3.0) inc=14;
	  else if(-ra< 6.0) inc=8;
	  else if(-ra<20.0) inc=4;
	  else if(-ra<40.0) inc=2;
	  if(setcol)
	    {
	      if (m==27)
                col=33;
	      else
		col=(int)(m);

	      if (col>33)
                col=33;

              col/=3;
	      setink(st->colors[col].pixel);
	    }

#ifdef USE_POLYGON
          {
            int n, nr;
	  for (n=0,nr=0;n<=sines-1;n+=inc,nr++)
	    {
	      track[nr].x=st->kugeln[i].x1+(int)(ra*st->sinus[n])+(st->kugeln[i].r1-ra)/2;
	      track[nr].y=st->kugeln[i].y1+(int)(ra*st->cosinus[n])+(st->kugeln[i].r1-ra)/2;
	    }
	  XFillPolygon(st->dpy,buf,st->gc,track,nr,Convex,CoordModeOrigin);
          }
#else /* Use XFillArc */
	  XFillArc(st->dpy, buf, st->gc,
		   (int)(st->kugeln[i].x1+(st->kugeln[i].r1+ra)/2),
		   (int)(st->kugeln[i].y1+(st->kugeln[i].r1+ra)/2),
		   (int)-(2*ra+1), (int)-(2*ra+1), 0, 360*64);
#endif
	}
    }
}
Esempio n. 10
0
static void dmooff (float offset, float fmax, float sdmo, int nx, float dx,
	int nt, float dt, float ft, float *vrms, float **ptx)
/*****************************************************************************
perform dmo in f-k domain for one offset
******************************************************************************
Input:
offset		source receiver offset
fmax		maximum frequency
sdmo		DMO stretch factor
nx		number of midpoints
dx		midpoint sampling interval
nt		number of time samples
dt		time sampling interval
ft		first time
vrms		array[nt] of rms velocities 
ptx		array[nx][nt] for p(t,x), zero-padded for fft (see notes)

Output:
ptx		array[nx][nt] for dmo-corrected p(t,x)
******************************************************************************
Notes:
To avoid having to allocate a separate work space that is larger than the
array ptx[nx][nt], ptx must be zero-padded to enable Fourier transform from x
to k via prime factor FFT.  nxpad (nx after zero-padding) can be estimated by
	nxpad = 2+npfar(nx+(int)(0.5*ABS(offset/dx)));
where npfar() is a function that returns a valid length for real-to-complex 
prime factor FFT.  ptx[nx] to ptx[nxfft-1] must point to zeros.
******************************************************************************
Author:	 Dave Hale, Colorado School of Mines, 08/08/91
*****************************************************************************/
{
	int nxfft,itmin,nu,nufft,nw,nk,ix,iu,iw,ik,it,iwn,
		iwmin,iwmax,nupad,ikmax;
	float dw,dk,tcon,wwscl,scale,scales,kmax,
		amp,phase,fr,fi,pwr,pwi,
		wmin,wmax,fftscl,du,fu,w,k,osdmo,*uoft,*tofu; 
	complex czero=cmplx(0.0,0.0),**ptk,*pu,*pw;

	/* number of cdps after padding for fft */
	nxfft = npfar(nx+(int)(0.5*ABS(offset/dx)));

	/* get minimum time of first non-zero sample */
	for (ix=0,itmin=nt; ix<nx; ++ix) {
		for (it=0; it<itmin && ptx[ix][it]==0.0; ++it);
		itmin = it;
	}
	
	/* if all zeros, simply return */
	if (itmin>=nt) return;
	
	/* make stretch and compress functions t(u) and u(t) */
	maketu(offset,itmin,fmax,nt,dt,ft,vrms,&uoft,&nu,&du,&fu,&tofu,&tcon);

	/* adjust DMO stretch factor for nominal error in log stretch; */
	/* solve sdmo*(sqrt(1-a/sdmo)-1) = 0.5*log(1-a), where a=0.5 */
	sdmo *= .62;

	/* inverse of dmo stretch factor */
	osdmo = 1.0/sdmo;

	/* maximum DMO shift (in samples) for any wavenumber k */
	nupad = 1.5*sdmo*tcon/du;
	
	/* frequency sampling */
	nufft = npfa(nu+nupad);
	nw = nufft;
	dw = 2.0*PI/(nufft*du);
	
	/* allocate workspace */
	pu = pw = ealloc1complex(nufft);
	
	/* wavenumber sampling and maximum wavenumber to apply dmo */
	nk = nxfft/2+1;
	dk = 2.0*PI/ABS(nxfft*dx);
	kmax = PI/ABS(dx);
	ikmax = NINT(kmax/dk);

	/* pointers to complex p(t,k) */
	ptk = (complex**)ealloc1(nk,sizeof(complex*));
	for (ik=0; ik<nk; ++ik)
		ptk[ik] = (complex*)ptx[0]+ik*nt;
	
	/* fft scale factor */
	fftscl = (float)nk/(float)(ikmax+1)/(nufft*nxfft);
	
	/* Fourier transform p(t,x) to p(t,k) */
	pfa2rc(-1,2,nt,nxfft,ptx[0],ptk[0]);

	/* loop over wavenumbers less than maximum */
	for (ik=0,k=0.0; ik<=ikmax; ++ik,k+=dk) {

		/* stretch p(t;k) to p(u) */
		ints8c(nt,dt,ft,ptk[ik],czero,czero,nu,tofu,pu);
		
		/* pad with zeros and Fourier transform p(u) to p(w) */
		for (iu=nu; iu<nufft; ++iu)
			pu[iu].r = pu[iu].i = 0.0;
		pfacc(1,nufft,pu);

		/* minimum and maximum frequencies to process */
		wmin = ABS(0.5*vrms[0]*k);
		wmax = ABS(PI/du);
		iwmin = MAX(1,MIN((nw-1)/2,NINT(wmin/dw)));
		iwmax = MAX(0,MIN((nw-1)/2,NINT(wmax/dw)));
		
		/* constant independent of w */
		wwscl = osdmo*pow(k*0.5*offset/tcon,2.0);
		
		/* zero dc (should be zero anyway) */
		pw[0].r = pw[0].i = 0.0;

		/* zero frequencies below minimum */
		for (iw=1,iwn=nw-iw; iw<iwmin; ++iw,--iwn)
			pw[iw].r = pw[iw].i = pw[iwn].r = pw[iwn].i = 0.0;
		
		/* do dmo between minimum and maximum frequencies */
		for (iw=iwmin,iwn=nw-iwmin,w=iwmin*dw; 
			iw<=iwmax; ++iw,--iwn,w+=dw) {
			scales = 1.0+wwscl/(w*w);
			scale = sqrt(scales);
			phase = sdmo*w*tcon*(scale-1.0);
			amp = fftscl*(1.0-sdmo+sdmo/scale);
			fr = amp*cos(phase);
			fi = amp*sin(phase);
			pwr = pw[iw].r;
			pwi = pw[iw].i;
			pw[iw].r = pwr*fr-pwi*fi;
			pw[iw].i = pwr*fi+pwi*fr;
			pwr = pw[iwn].r;
			pwi = pw[iwn].i;
			pw[iwn].r = pwr*fr+pwi*fi;
			pw[iwn].i = pwi*fr-pwr*fi;
		}

		/* zero frequencies above maximum to Nyquist (if present) */
		for (iw=iwmax+1,iwn=nw-iw; iw<=nw/2; ++iw,--iwn)
			pw[iw].r = pw[iw].i = pw[iwn].r = pw[iwn].i = 0.0;
		
		/* Fourier transform p(w) to p(u) */
		pfacc(-1,nufft,pu);
		
		/* compress p(u) to p(t;k) */
		ints8c(nu,du,fu,pu,czero,czero,nt,uoft,ptk[ik]);
	}

	/* zero wavenumber between maximum and Nyquist */
	for (; ik<nk; ++ik)
		for (it=0; it<nt; ++it)
			ptk[ik][it].r = ptk[ik][it].i = 0.0;
	
	/* Fourier transform p(t,k) to p(t,x) */
	pfa2cr(1,2,nt,nxfft,ptk[0],ptx[0]);
	
	/* free workspace */
	free1float(tofu);
	free1float(uoft);
	free1complex(pu);
	free1(ptk);
}
Esempio n. 11
0
int
main(int argc, char **argv)
{
	int nt;		/* number of time samples per trace */
	float dt;	/* time sampling interval */
	float ft;	/* time of first sample */
	int it;		/* time sample index */
	int cdpmin;	/* minimum cdp to process */
	int cdpmax;	/* maximum cdp to process */
	float dx;	/* cdp sampling interval */
	int nx;		/* number of cdps to process */
	int nxfft;	/* number of cdps after zero padding for fft */
	int nxpad;	/* minimum number of cdps for zero padding */
	int ix;		/* cdp index, starting with ix=0 */
	int noffmix;	/* number of offsets to mix */
	float *tdmo;	/* times at which rms velocities are specified */
	float *vdmo;	/* rms velocities at times specified in tdmo */
	float sdmo;	/* DMO stretch factor */
	int ntdmo;	/* number tnmo values specified */
	int itdmo;	/* index into tnmo array */
	int nvdmo;	/* number vnmo values specified */
	float fmax;	/* maximum frequency */
	float *vrms;	/* uniformly sampled vrms(t) */
	float **p;	/* traces for one offset - common-offset gather */
	float **q;	/* DMO-corrected and mixed traces to be output */
	float offset;	/* source-receiver offset of current trace */
	float oldoffset;/* offset of previous trace */
	int noff;	/* number of offsets processed in current mix */
	int ntrace;	/* number of traces processed in current mix */
	int itrace;	/* trace index */
	int gottrace;	/* non-zero if an input trace was read */
	int done;	/* non-zero if done */
	int verbose;	/* =1 for diagnostic print */
	char *tmpdir;	/* directory path for tmp files	*/
	cwp_Bool istmpdir=cwp_false;/* true for user given path */

	/* hook up getpar */
	initargs(argc, argv);
	requestdoc(1);

	/* get information from the first header */
	if (!gettr(&tr)) err("can't get first trace");
	nt = tr.ns;
	dt = ((double) tr.dt)/1000000.0;
	ft = tr.delrt/1000.0;
	offset = tr.offset;

	/* get parameters */
	if (!getparint("cdpmin",&cdpmin)) err("must specify cdpmin");
	if (!getparint("cdpmax",&cdpmax)) err("must specify cdpmax");
	if (cdpmin>cdpmax) err("cdpmin must not be greater than cdpmax");
	if (!getparfloat("dxcdp",&dx)) err("must specify dxcdp");
	if (!getparint("noffmix",&noffmix)) err("must specify noffmix");
	ntdmo = countparval("tdmo");
	if (ntdmo==0) ntdmo = 1;
	tdmo = ealloc1float(ntdmo);
	if (!getparfloat("tdmo",tdmo)) tdmo[0] = 0.0;
	nvdmo = countparval("vdmo");
	if (nvdmo==0) nvdmo = 1;
	if (nvdmo!=ntdmo) err("number of tdmo and vdmo must be equal");
	vdmo = ealloc1float(nvdmo);
	if (!getparfloat("vdmo",vdmo)) vdmo[0] = 1500.0;
	for (itdmo=1; itdmo<ntdmo; ++itdmo)
		if (tdmo[itdmo]<=tdmo[itdmo-1])
			err("tdmo must increase monotonically");
	if (!getparfloat("sdmo",&sdmo)) sdmo = 1.0;
	if (!getparfloat("fmax",&fmax)) fmax = 0.5/dt;
	if (!getparint("verbose",&verbose)) verbose=0;
	
	/* Look for user-supplied tmpdir */
	if (!getparstring("tmpdir",&tmpdir) &&
	    !(tmpdir = getenv("CWP_TMPDIR"))) tmpdir="";
	if (!STREQ(tmpdir, "") && access(tmpdir, WRITE_OK))
		err("you can't write in %s (or it doesn't exist)", tmpdir);
	
	/* make uniformly sampled rms velocity function of time */
	vrms = ealloc1float(nt);
	mkvrms(ntdmo,tdmo,vdmo,nt,dt,ft,vrms);
	
	/* determine number of cdps to process */
	nx = cdpmax-cdpmin+1;
	
	/* allocate and zero common-offset gather p(t,x) */
	nxpad = 0.5*ABS(offset/dx);
	nxfft = npfar(nx+nxpad);
	p = ealloc2float(nt,nxfft+2);
	for (ix=0; ix<nxfft; ++ix)
		for (it=0; it<nt; ++it)
			p[ix][it] = 0.0;

	/* allocate and zero offset mix accumulator q(t,x) */
	q = ealloc2float(nt,nx);
	for (ix=0; ix<nx; ++ix)
		for (it=0; it<nt; ++it)
			q[ix][it] = 0.0;
		
	/* open temporary file for headers */
	if (STREQ(tmpdir,"")) {
		headerfp = etmpfile();
		if (verbose) warn("using tmpfile() call");
	} else { /* user-supplied tmpdir */
		char directory[BUFSIZ];
		strcpy(directory, tmpdir);
		strcpy(headerfile, temporary_filename(directory));
		/* Trap signals so can remove temp files */
		signal(SIGINT,  (void (*) (int)) closefiles);
		signal(SIGQUIT, (void (*) (int)) closefiles);
		signal(SIGHUP,  (void (*) (int)) closefiles);
		signal(SIGTERM, (void (*) (int)) closefiles);
		headerfp = efopen(headerfile, "w+");
      		istmpdir=cwp_true;		
		if (verbose)
			warn("putting temporary header file in %s", directory);
	}

	/* initialize */
	oldoffset = offset;
	gottrace = 1;
	done = 0;
	ntrace = 0;
	noff = 0;

	/* loop over traces */
	do {
		/* if got a trace, determine offset */
		if (gottrace) offset = tr.offset;
		
		/* if an offset is complete */
		if ((gottrace && offset!=oldoffset) || !gottrace) {
		
			/* do dmo for old common-offset gather */
			dmooff(oldoffset,fmax,sdmo,nx,dx,nt,dt,ft,vrms,p);
			
			/* add dmo-corrected traces to mix */
			for (ix=0; ix<nx; ++ix)
				for (it=0; it<nt; ++it)
					q[ix][it] += p[ix][it];
			
			/* count offsets in mix */
			noff++;
							
			/* free space for old common-offset gather */
			free2float(p);
			
			/* if beginning a new offset */
			if (offset!=oldoffset) {
				
				/* allocate space for new offset */
				nxpad = 0.5*ABS(offset/dx);
				nxfft = npfar(nx+nxpad);
				p = ealloc2float(nt,nxfft+2);
				for (ix=0; ix<nxfft; ++ix)
					for (it=0; it<nt; ++it)
						p[ix][it] = 0.0;
			}
		}
		
		/* if a mix of offsets is complete */
		if (noff==noffmix || !gottrace) {
			
			/* rewind trace header file */
			erewind(headerfp);
			
			/* loop over all output traces */
			for (itrace=0; itrace<ntrace; ++itrace) {
			
				/* read trace header and determine cdp index */
				efread(&tro,HDRBYTES,1,headerfp);
				
				/* get dmo-corrected data */
				memcpy( (void *) tro.data,
					(const void *)	q[tro.cdp-cdpmin],
					nt*sizeof(float));
				
				/* write output trace */
				puttr(&tro);
			}
			
			/* report */
			if (verbose) 
				fprintf(stderr,"\tCompleted mix of "
					"%d offsets with %d traces\n",
					noff,ntrace);
			
			/* if no more traces, break */
			if (!gottrace) break;
			
			/* rewind trace header file */
			erewind(headerfp);

			/* reset number of offsets and traces in mix */
			noff = 0;
			ntrace = 0;
			
			/* zero offset mix accumulator */
			for (ix=0; ix<nx; ++ix)
				for (it=0; it<nt; ++it)
					q[ix][it] = 0.0;
		}
			
		/* if cdp is within range to process */
		if (tr.cdp>=cdpmin && tr.cdp<=cdpmax) {
	
			/* save trace header and update number of traces */
			efwrite(&tr,HDRBYTES,1,headerfp);
			ntrace++;

			/* remember offset */
			oldoffset = offset;

			/* get trace samples */
			memcpy( (void *) p[tr.cdp-cdpmin],
				   (const void *) tr.data, nt*sizeof(float));
		}

		/* get next trace (if there is one) */
		if (!gettr(&tr)) gottrace = 0;
		
	} while (!done);

	/* clean up */
	efclose(headerfp);
	if (istmpdir) eremove(headerfile);
	return(CWP_Exit());
}
static gboolean
keynav_failed (GtkIconView        *current_view,
               GtkDirectionType    direction,
               CinnamonControlCenter *shell)
{
  GList *views, *v;
  GtkIconView *new_view;
  GtkTreePath *path;
  GtkTreeModel *model;
  GtkTreeIter iter;
  gint col, c, dist, d;
  GtkTreePath *sel;
  gboolean res;

  res = FALSE;

  views = get_item_views (shell);

  for (v = views; v; v = v->next)
    {
      if (v->data == current_view)
        break;
    }

  if (direction == GTK_DIR_DOWN && v != NULL && v->next != NULL)
    {
      new_view = v->next->data;

      if (gtk_icon_view_get_cursor (current_view, &path, NULL))
        {
          col = gtk_icon_view_get_item_column (current_view, path);
          gtk_tree_path_free (path);

          sel = NULL;
          dist = 1000;
          model = gtk_icon_view_get_model (new_view);
          gtk_tree_model_get_iter_first (model, &iter);
          do {
            path = gtk_tree_model_get_path (model, &iter);
            c = gtk_icon_view_get_item_column (new_view, path);
            d = ABS (c - col);
            if (d < dist)
              {
                if (sel)
                  gtk_tree_path_free (sel);
                sel = path;
                dist = d;
              }
            else
              gtk_tree_path_free (path);
          } while (gtk_tree_model_iter_next (model, &iter));

          gtk_icon_view_set_cursor (new_view, sel, NULL, FALSE);
          gtk_tree_path_free (sel);
        }

      gtk_widget_grab_focus (GTK_WIDGET (new_view));

      res = TRUE;
    }

  if (direction == GTK_DIR_UP && v != NULL && v->prev != NULL)
    {
      new_view = v->prev->data;

      if (gtk_icon_view_get_cursor (current_view, &path, NULL))
        {
          col = gtk_icon_view_get_item_column (current_view, path);
          gtk_tree_path_free (path);

          sel = NULL;
          dist = 1000;
          model = gtk_icon_view_get_model (new_view);
          gtk_tree_model_get_iter_first (model, &iter);
          do {
            path = gtk_tree_model_get_path (model, &iter);
            c = gtk_icon_view_get_item_column (new_view, path);
            d = ABS (c - col);
            if (d <= dist)
              {
                if (sel)
                  gtk_tree_path_free (sel);
                sel = path;
                dist = d;
              }
            else
              gtk_tree_path_free (path);
          } while (gtk_tree_model_iter_next (model, &iter));

          gtk_icon_view_set_cursor (new_view, sel, NULL, FALSE);
          gtk_tree_path_free (sel);
        }

      gtk_widget_grab_focus (GTK_WIDGET (new_view));

      res = TRUE;
    }

  g_list_free (views);

  return res;
}
Esempio n. 13
0
size_t
mpz_inp_raw (mpz_ptr x, FILE *fp)
{
  unsigned char  csize_bytes[4];
  mp_size_t      csize, abs_xsize, i;
  size_t         abs_csize;
  char           *cp;
  mp_ptr         xp, sp, ep;
  mp_limb_t      slimb, elimb;

  if (fp == 0)
    fp = stdin;

  /* 4 bytes for size */
  if (fread (csize_bytes, sizeof (csize_bytes), 1, fp) != 1)
    return 0;

  csize =
    (  (mp_size_t) csize_bytes[0] << 24)
    + ((mp_size_t) csize_bytes[1] << 16)
    + ((mp_size_t) csize_bytes[2] << 8)
    + ((mp_size_t) csize_bytes[3]);

  /* Sign extend if necessary.
     Could write "csize -= ((csize & 0x80000000L) << 1)", but that tickles a
     bug in gcc 3.0 for powerpc64 on AIX.  */
  if (sizeof (csize) > 4 && csize & 0x80000000L)
    csize -= 0x80000000L << 1;

  abs_csize = ABS (csize);

  /* round up to a multiple of limbs */
  abs_xsize = BITS_TO_LIMBS (abs_csize*8);

  if (abs_xsize != 0)
    {
      xp = MPZ_NEWALLOC (x, abs_xsize);

      /* Get limb boundaries right in the read, for the benefit of the
	 non-nails case.  */
      xp[0] = 0;
      cp = (char *) (xp + abs_xsize) - abs_csize;
      if (fread (cp, abs_csize, 1, fp) != 1)
	return 0;

      if (GMP_NAIL_BITS == 0)
	{
	  /* Reverse limbs to least significant first, and byte swap.  If
	     abs_xsize is odd then on the last iteration elimb and slimb are
	     the same.  It doesn't seem extra code to handle that case
	     separately, to save an NTOH.  */
	  sp = xp;
	  ep = xp + abs_xsize-1;
	  for (i = 0; i < (abs_xsize+1)/2; i++)
	    {
	      NTOH_LIMB_FETCH (elimb, ep);
	      NTOH_LIMB_FETCH (slimb, sp);
	      *sp++ = elimb;
	      *ep-- = slimb;
	    }
	}
      else
	{
	  /* It ought to be possible to do the transformation in-place, but
	     for now it's easier to use an extra temporary area.  */
	  mp_limb_t  byte, limb;
	  int	     bits;
	  mp_size_t  tpos;
	  mp_ptr     tp;
	  TMP_DECL;

	  TMP_MARK;
	  tp = TMP_ALLOC_LIMBS (abs_xsize);
	  limb = 0;
	  bits = 0;
	  tpos = 0;
	  for (i = abs_csize-1; i >= 0; i--)
	    {
	      byte = (unsigned char) cp[i];
	      limb |= (byte << bits);
	      bits += 8;
	      if (bits >= GMP_NUMB_BITS)
		{
		  ASSERT (tpos < abs_xsize);
		  tp[tpos++] = limb & GMP_NUMB_MASK;
		  bits -= GMP_NUMB_BITS;
		  ASSERT (bits < 8);
		  limb = byte >> (8 - bits);
		}
	    }
	  if (bits != 0)
	    {
	      ASSERT (tpos < abs_xsize);
	      tp[tpos++] = limb;
	    }
	  ASSERT (tpos == abs_xsize);

	  MPN_COPY (xp, tp, abs_xsize);
	  TMP_FREE;
	}
Esempio n. 14
0
void WSAMovie_v1::displayFrame(int frameNum, int pageNum, int x, int y, uint16 flags, const uint8 *table1, const uint8 *table2) {
	if (frameNum >= _numFrames || frameNum < 0 || !_opened)
		return;

	_x = x;
	_y = y;
	_drawPage = pageNum;

	uint8 *dst = 0;
	if (_flags & WF_OFFSCREEN_DECODE)
		dst = _offscreenBuffer;
	else
		dst = _screen->getPageRect(_drawPage, _x, _y, _width, _height);

	if (_currentFrame == _numFrames) {
		if (!(_flags & WF_NO_FIRST_FRAME)) {
			if (_flags & WF_OFFSCREEN_DECODE)
				Screen::decodeFrameDelta(dst, _deltaBuffer);
			else
				Screen::decodeFrameDeltaPage(dst, _deltaBuffer, _width, (_flags & WF_XOR) == 0);
		}
		_currentFrame = 0;
	}

	// try to reduce the number of needed frame operations
	int diffCount = ABS(_currentFrame - frameNum);
	int frameStep = 1;
	int frameCount;
	if (_currentFrame < frameNum) {
		frameCount = _numFrames - frameNum + _currentFrame;
		if (diffCount > frameCount && !(_flags & WF_NO_LAST_FRAME))
			frameStep = -1;
		else
			frameCount = diffCount;
	} else {
		frameCount = _numFrames - _currentFrame + frameNum;
		if (frameCount >= diffCount || (_flags & WF_NO_LAST_FRAME)) {
			frameStep = -1;
			frameCount = diffCount;
		}
	}

	// process
	if (frameStep > 0) {
		uint16 cf = _currentFrame;
		while (frameCount--) {
			cf += frameStep;
			processFrame(cf, dst);
			if (cf == _numFrames)
				cf = 0;
		}
	} else {
		uint16 cf = _currentFrame;
		while (frameCount--) {
			if (cf == 0)
				cf = _numFrames;
			processFrame(cf, dst);
			cf += frameStep;
		}
	}

	// display
	_currentFrame = frameNum;
	if (_flags & WF_OFFSCREEN_DECODE) {
		int pageBackUp = _screen->setCurPage(_drawPage);

		int plotFunc = (flags & 0xFF00) >> 12;
		int unk1 = flags & 0xFF;

		_screen->copyWsaRect(_x, _y, _width, _height, 0, plotFunc, _offscreenBuffer, unk1, table1, table2);

		_screen->_curPage = pageBackUp;
	}
Esempio n. 15
0
void draw_line( int x1, int y1, int x2, int y2, char value ) {
#if 0
	if ( x1 == x2 ) {
		//Draw vertical line
		unsigned char i;
		for ( i = y1; i <= y2; i++ ) {
			draw_char( x1, i, value );
		}
	}
	else if ( y1 == y2 ) {
		//Draw horizontal line
		unsigned char i;
		for ( i = x1; i <= x2; i++ ) {
			draw_char( i, y1, value );
		}
	}
	else {
		// Figure out octant
		unsigned int oct;
		float g = ( (float)y2 - y1 ) / ( (float)x2 - x1 );

		if ( x2 > x1 ) {
			if ( g > 1 ) {
				oct = 1;
			}
			else if ( g <= 1 && g > 0 ) {
				oct = 0;
			}
			else if ( g <= 0 && g > -1 ) {
				oct = 7;
			}
			else {
				oct = 6;
			}
		}
		else {
			if ( g > 1 ) {
				oct = 5;
			}
			else if ( g <= 1 && g > 0 ) {
				oct = 4;
			}
			else if ( g <= 0 && g > -1 ) {
				oct = 3;
			}
			else {
				oct = 2;
			}
		}

		// Translate octants to settings
		unsigned int useX;
		switch ( oct ) {
		case 0:
		case 3:
		case 4:
		case 7:
			useX = 1;
			break;
		case 1:
		case 2:
		case 5:
		case 6:
			useX = 0;
			break;

		}

		unsigned int i1, i, i2, c;
		float dx = (float)x2 - x1, dy = (float)y2 - y1, m;

		if ( useX ) {
			m = dy / dx;
			c = y1; i1 = x1; i2 = x2;
		}
		else {
			m = dx / dy;
			c = x1; i1 = y1; i2 = y2;
		}

		int flipM;

		switch ( oct ) {
		case 0:
		case 1:
		case 2:
		case 7:
			flipM = 1;
			break;
		case 3:
		case 4:
		case 5:
		case 6:
			flipM = -1;
			break;
		}

		// Perform the actual loop
		float err = 0.0;

		for ( i = i1; ( i2 > i1 ) ? i <= i2 : i >= i2; ( i2 > i1 ) ? i++ : i-- ) {
			( useX ) ? draw_char( i, c, value ) : draw_char( c, i, value );
			err += ABS( m );

			if ( err > 0.5f ) {
				( SIGN( m*flipM ) > 0 ) ? c++ : c--;
				err -= 1.0f;
			}
		}
	}
#else
	if ( x1 == x2 ) {
		// Draw vertical line
		for ( int i = y1; ( y2 > y1 ) ? i <= y2 : i >= y2; ( y2 > y1 ) ? i++ : i-- ) {
			draw_char( x1, i, value );
		}
	}
	else if ( y1 == y2 ) {
		// Draw horizontal line
		for ( int i = x1; ( x2 > x1 ) ? i <= x2 : i >= x2; ( x2 > x1 ) ? i++ : i-- ) {
			draw_char( i, y1, value );
		}
	}
	else {
		// Get Bresenhaming...
		float dx = x2-x1;
		float dy = y2-y1;
		float err = 0.0;
		float derr = ABS( dy/dx );

		for ( int x = x1, y = y1; ( dx > 0 ) ? x <= x2 : x >= x2; ( dx > 0 ) ? x++ : x-- ) {
			draw_char( x, y, value );
			err += derr;
			while ( err >= 0.5 && ( ( dy > 0 ) ? y <= y2 : y >= y2 ) ) {
				draw_char( x, y, value );
				y += ( dy > 0 ) - ( dy < 0 );

				err -= 1.0;
			}
		}
	}
#endif
}
Esempio n. 16
0
static int
_drawlinewidth (SDL_Surface* surface, SDL_Rect *cliprect, Uint32 color,
    int *pts, int width)
{
    int loop;
    int xinc = 0, yinc = 0;
    int newpts[4], tmp[4], range[4];
    int anydrawn = 0;
    
    if (ABS (pts[0] - pts[2]) > ABS (pts[1] - pts[3]))
        yinc = 1;
    else
        xinc = 1;
    
    if (_clipline (cliprect, pts[0], pts[1], pts[2], pts[3], newpts))
    {
        if (newpts[1] == newpts[3])
            _drawhorzline (surface, color, newpts[0], newpts[1], newpts[2]);
        else if (pts[0] == pts[2])
            _drawvertline (surface, color, newpts[0], newpts[1], newpts[3]);
        else
            _drawline (surface, color, newpts[0], newpts[1], newpts[2],
                newpts[3]);
        
        anydrawn = 1;
        memcpy (range, newpts, sizeof(int) * 4);
    }
    else
    {
        range[0] = range[1] = 10000;
        range[2] = range[3] = -10000;
    }
    
    for (loop = 1; loop < width; loop += 2)
    {
        newpts[0] = pts[0] + xinc*(loop/2+1);
        newpts[1] = pts[1] + yinc*(loop/2+1);
        newpts[2] = pts[2] + xinc*(loop/2+1);
        newpts[3] = pts[3] + yinc*(loop/2+1);
        
        if(_clipline (cliprect, newpts[0], newpts[1], newpts[2], newpts[3],
                tmp))
        {
            memcpy (newpts, tmp, sizeof(int)*4);
            if (newpts[1] == newpts[3])
                _drawhorzline (surface, color, newpts[0], newpts[1], newpts[2]);
            else if (pts[0] == pts[2])
                _drawvertline (surface, color, newpts[0], newpts[1], newpts[3]);
            else
                _drawline (surface, color, newpts[0], newpts[1], newpts[2],
                    newpts[3]);
            
            anydrawn = 1;
            range[0] = MIN(newpts[0], range[0]);
            range[1] = MIN(newpts[1], range[1]);
            range[2] = MAX(newpts[2], range[2]);
            range[3] = MAX(newpts[3], range[3]);
        }

        if(loop + 1 < width)
        {
            newpts[0] = pts[0] - xinc*(loop/2+1);
            newpts[1] = pts[1] - yinc*(loop/2+1);
            newpts[2] = pts[2] - xinc*(loop/2+1);
            newpts[3] = pts[3] - yinc*(loop/2+1);
            if (_clipline (cliprect, newpts[0], newpts[1], newpts[2], newpts[3],
                    tmp))
            {
                memcpy (newpts, tmp, sizeof(int)*4);
                if (newpts[1] == newpts[3])
                    _drawhorzline (surface, color, newpts[0], newpts[1],
                        newpts[2]);
                else if (pts[0] == pts[2])
                    _drawvertline (surface, color, newpts[0], newpts[1],
                        newpts[3]);
                else
                    _drawline (surface, color, newpts[0], newpts[1], newpts[2],
                        newpts[3]);

                anydrawn = 1;
                range[0] = MIN(newpts[0], range[0]);
                range[1] = MIN(newpts[1], range[1]);
                range[2] = MAX(newpts[2], range[2]);
                range[3] = MAX(newpts[3], range[3]);
            }
        }
    }

    if (anydrawn)
        memcpy (pts, range, sizeof(int)*4);
    return anydrawn;
}
Esempio n. 17
0
void cmonot(size_t n, float x[], float y[], float yd[][4])
/*****************************************************************************
compute cubic interpolation coefficients via the Fritsch-Carlson method,
which preserves monotonicity
******************************************************************************
Input:
n		number of samples
x  		array[n] of monotonically increasing or decreasing abscissae
y		array[n] of ordinates

Output:
yd		array[n][4] of cubic interpolation coefficients (see notes)
******************************************************************************
Notes:
The computed cubic spline coefficients are as follows:
yd[i][0] = y(x[i])    (the value of y at x = x[i])
yd[i][1] = y'(x[i])   (the 1st derivative of y at x = x[i])
yd[i][2] = y''(x[i])  (the 2nd derivative of y at x = x[i])
yd[i][3] = y'''(x[i]) (the 3rd derivative of y at x = x[i])

To evaluate y(x) for x between x[i] and x[i+1] and h = x-x[i],
use the computed coefficients as follows:
y(x) = yd[i][0]+h*(yd[i][1]+h*(yd[i][2]/2.0+h*yd[i][3]/6.0))

The Fritsch-Carlson method yields continuous 1st derivatives, but 2nd
and 3rd derivatives are discontinuous.  The method will yield a
monotonic interpolant for monotonic data.  1st derivatives are set
to zero wherever first divided differences change sign.

For more information, see Fritsch, F. N., and Carlson, R. E., 1980,
Monotone piecewise cubic interpolation:  SIAM J. Numer. Anal., v. 17,
n. 2, p. 238-246.

Also, see the book by Kahaner, D., Moler, C., and Nash, S., 1989,
Numerical Methods and Software, Prentice Hall.  This function was
derived from SUBROUTINE PCHEZ contained on the diskette that comes
with the book.
******************************************************************************
Author:  Dave Hale, Colorado School of Mines, 09/30/89
Modified:  Dave Hale, Colorado School of Mines, 02/28/91
    changed to work for n=1.
Modified:  Dave Hale, Colorado School of Mines, 08/04/91
    fixed bug in computation of left end derivative
*****************************************************************************/
{
	size_t i;
	float h1, h2, del1, del2, dmin, dmax, hsum, hsum3, w1, w2, drat1, drat2, divdf3;

	/* copy ordinates into output array */
	for (i = 0; i < n; i++) yd[i][0] = y[i];

	/* if n=1, then use constant interpolation */
	if (n == 1) {
		yd[0][1] = 0.0;
		yd[0][2] = 0.0;
		yd[0][3] = 0.0;
		return;

		/* else, if n=2, then use linear interpolation */
	} else if (n == 2) {
		yd[0][1] = yd[1][1] = (y[1] - y[0]) / (x[1] - x[0]);
		yd[0][2] = yd[1][2] = 0.0;
		yd[0][3] = yd[1][3] = 0.0;
		return;
	}

	/* set left end derivative via shape-preserving 3-point formula */
	h1       = x[1] - x[0];
	h2       = x[2] - x[1];
	hsum     = h1 + h2;
	del1     = (y[1] - y[0]) / h1;
	del2     = (y[2] - y[1]) / h2;
	w1       = (h1 + hsum) / hsum;
	w2       = -h1 / hsum;
	yd[0][1] = w1 * del1 + w2 * del2;
	if (yd[0][1] * del1 <= 0.0)
		yd[0][1] = 0.0;
	else if (del1 * del2 < 0.0) {
		dmax                                    = 3.0 * del1;
		if (ABS(yd[0][1]) > ABS(dmax)) yd[0][1] = dmax;
	}

	/* loop over interior points */
	for (i = 1; i < n - 1; i++) {

		/* compute intervals and slopes */
		h1   = x[i] - x[i - 1];
		h2   = x[i + 1] - x[i];
		hsum = h1 + h2;
		del1 = (y[i] - y[i - 1]) / h1;
		del2 = (y[i + 1] - y[i]) / h2;

		/* if not strictly monotonic, zero derivative */
		if (del1 * del2 <= 0.0) {
			yd[i][1] = 0.0;

			/*
			 * else, if strictly monotonic, use Butland's formula:
			 *      3*(h1+h2)*del1*del2
			 * -------------------------------
			 * ((2*h1+h2)*del1+(h1+2*h2)*del2)
			 * computed as follows to avoid roundoff error
			 */
		} else {
			hsum3    = hsum + hsum + hsum;
			w1       = (hsum + h1) / hsum3;
			w2       = (hsum + h2) / hsum3;
			dmin     = MIN(ABS(del1), ABS(del2));
			dmax     = MAX(ABS(del1), ABS(del2));
			drat1    = del1 / dmax;
			drat2    = del2 / dmax;
			yd[i][1] = dmin / (w1 * drat1 + w2 * drat2);
		}
	}

	/* set right end derivative via shape-preserving 3-point formula */
	w1           = -h2 / hsum;
	w2           = (h2 + hsum) / hsum;
	yd[n - 1][1] = w1 * del1 + w2 * del2;
	if (yd[n - 1][1] * del2 <= 0.0)
		yd[n - 1][1] = 0.0;
	else if (del1 * del2 < 0.0) {
		dmax                                            = 3.0 * del2;
		if (ABS(yd[n - 1][1]) > ABS(dmax)) yd[n - 1][1] = dmax;
	}

	/* compute 2nd and 3rd derivatives of cubic polynomials */
	for (i = 0; i < n - 1; i++) {
		h2       = x[i + 1] - x[i];
		del2     = (y[i + 1] - y[i]) / h2;
		divdf3   = yd[i][1] + yd[i + 1][1] - 2.0 * del2;
		yd[i][2] = 2.0 * (del2 - yd[i][1] - divdf3) / h2;
		yd[i][3] = (divdf3 / h2) * (6.0 / h2);
	}
	yd[n - 1][2] = yd[n - 2][2] + (x[n - 1] - x[n - 2]) * yd[n - 2][3];
	yd[n - 1][3] = yd[n - 2][3];
}
Esempio n. 18
0
/*---------------------------------------------------------------------------*/
static void
ct_callback(void *ptr)
{
  uint32_t wind_speed;
  int16_t wind_dir;
  int16_t wind_dir_delta;

  /* Disable to make the calculations in an interrupt-safe context */
  GPIO_DISABLE_INTERRUPT(ANEMOMETER_SENSOR_PORT_BASE,
                         ANEMOMETER_SENSOR_PIN_MASK);
  wind_speed = weather_sensors.anemometer.ticks;
  wind_speed *= WEATHER_METER_ANEMOMETER_SPEED_1S;
  weather_sensors.anemometer.value = (uint16_t)wind_speed;
  anemometer.ticks_avg++;
  anemometer.value_avg += weather_sensors.anemometer.value;
  anemometer.value_buf_xm += weather_sensors.anemometer.value;

  /* Take maximum value */
  if(weather_sensors.anemometer.value > anemometer.value_max) {
    anemometer.value_max = weather_sensors.anemometer.value;
  }

  /* Mitsuta method to get the wind direction average */
  wind_dir = weather_meter_get_wind_dir();
  wind_dir_delta = wind_dir - wind_vane.value_prev;

  if(wind_dir_delta < -1800) {
    wind_vane.value_prev += wind_dir_delta + 3600;
  } else if(wind_dir_delta > 1800) {
    wind_vane.value_prev += wind_dir_delta - 3600;
  } else {
    wind_vane.value_prev += wind_dir_delta;
  }

  wind_vane.value_buf_xm += wind_vane.value_prev;

  /* Calculate the 2 minute average */
  if(!(anemometer.ticks_avg % WEATHER_METER_AVG_PERIOD)) {
    PRINTF("\nWeather: calculate the %u averages ***\n", WEATHER_METER_AVG_PERIOD);

    if(anemometer.value_buf_xm) {
      anemometer.value_avg_xm = anemometer.value_buf_xm / WEATHER_METER_AVG_PERIOD;
      anemometer.value_buf_xm = 0;
    } else {
      anemometer.value_avg_xm = 0;
    }

    if(wind_vane.value_buf_xm >= 0) {
      wind_vane.value_buf_xm = wind_vane.value_buf_xm / WEATHER_METER_AVG_PERIOD;
      wind_vane.value_avg_xm = wind_vane.value_buf_xm;
    } else {
      wind_vane.value_buf_xm = ABS(wind_vane.value_buf_xm) / WEATHER_METER_AVG_PERIOD;
      wind_vane.value_avg_xm = wind_vane.value_buf_xm;
      wind_vane.value_avg_xm = ~wind_vane.value_avg_xm + 1;
    }

    if(wind_vane.value_avg_xm >= 3600) {
      wind_vane.value_avg_xm -= 3600;
    } else if(wind_vane.value_avg_xm < 0) {
      wind_vane.value_avg_xm += 3600;
    }

    wind_vane.value_buf_xm = 0;
    wind_vane.value_prev = wind_dir;
  }

  /* Check for roll-over */
  if(!anemometer.ticks_avg) {
    anemometer.value_avg = 0;
  }

  weather_sensors.anemometer.ticks = 0;

  /* Enable the interrupt again */
  GPIO_ENABLE_INTERRUPT(ANEMOMETER_SENSOR_PORT_BASE,
                        ANEMOMETER_SENSOR_PIN_MASK);

  ctimer_set(&ct, CLOCK_SECOND, ct_callback, NULL);
}
Esempio n. 19
0
  static FT_Error
  ftc_sbit_node_load( FTC_SBitNode    snode,
                      FTC_Manager     manager,
                      FTC_SBitFamily  sfam,
                      FT_UInt         gindex,
                      FT_ULong       *asize )
  {
    FT_Error       error;
    FTC_GlyphNode  gnode = FTC_GLYPH_NODE( snode );
    FT_Memory      memory;
    FT_Face        face;
    FT_Size        size;
    FTC_SBit       sbit;


    if ( gindex <  (FT_UInt)gnode->item_start                     ||
         gindex >= (FT_UInt)gnode->item_start + gnode->item_count )
    {
      FT_ERROR(( "ftc_sbit_node_load: invalid glyph index" ));
      return FTC_Err_Invalid_Argument;
    }

    memory = manager->library->memory;

    sbit = snode->sbits + ( gindex - gnode->item_start );

    error = FTC_Manager_Lookup_Size( manager, &sfam->type.font,
                                     &face, &size );
    if ( !error )
    {
      /* by default, indicates a `missing' glyph */
      sbit->buffer = 0;

      error = FT_Load_Glyph( face, gindex, sfam->type.flags | FT_LOAD_RENDER );
      if ( !error )
      {
        FT_Int        temp;
        FT_GlyphSlot  slot   = face->glyph;
        FT_Bitmap*    bitmap = &slot->bitmap;
        FT_Int        xadvance, yadvance;


        /* check that our values fit into 8-bit containers!       */
        /* If this is not the case, our bitmap is too large       */
        /* and we will leave it as `missing' with sbit.buffer = 0 */

#define CHECK_CHAR( d )  ( temp = (FT_Char)d, temp == d )
#define CHECK_BYTE( d )  ( temp = (FT_Byte)d, temp == d )

        /* XXX: FIXME: add support for vertical layouts maybe */

        /* horizontal advance in pixels */
        xadvance = ( slot->metrics.horiAdvance + 32 ) >> 6;
        yadvance = ( slot->metrics.vertAdvance + 32 ) >> 6;

        if ( CHECK_BYTE( bitmap->rows  )     &&
             CHECK_BYTE( bitmap->width )     &&
             CHECK_CHAR( bitmap->pitch )     &&
             CHECK_CHAR( slot->bitmap_left ) &&
             CHECK_CHAR( slot->bitmap_top  ) &&
             CHECK_CHAR( xadvance )          &&
             CHECK_CHAR( yadvance )          )
        {
          sbit->width     = (FT_Byte)bitmap->width;
          sbit->height    = (FT_Byte)bitmap->rows;
          sbit->pitch     = (FT_Char)bitmap->pitch;
          sbit->left      = (FT_Char)slot->bitmap_left;
          sbit->top       = (FT_Char)slot->bitmap_top;
          sbit->xadvance  = (FT_Char)xadvance;
          sbit->yadvance  = (FT_Char)yadvance;
          sbit->format    = (FT_Byte)bitmap->pixel_mode;
          sbit->max_grays = (FT_Byte)(bitmap->num_grays - 1);

#if 0 /* this doesn't work well with embedded bitmaps */

          /* grab the bitmap when possible - this is a hack! */
          if ( slot->internal->flags & FT_GLYPH_OWN_BITMAP )
          {
            slot->internal->flags &= ~FT_GLYPH_OWN_BITMAP;
            sbit->buffer           = bitmap->buffer;
          }
          else
#endif
          {
            /* copy the bitmap into a new buffer -- ignore error */
            error = ftc_sbit_copy_bitmap( sbit, bitmap, memory );
          }

          /* now, compute size */
          if ( asize )
            *asize = ABS( sbit->pitch ) * sbit->height;

        }  /* glyph dimensions ok */

      } /* glyph loading successful */
Esempio n. 20
0
static GstEvent *
gst_rg_volume_tag_event (GstRgVolume * self, GstEvent * event)
{
  GstTagList *tag_list;
  gboolean has_track_gain, has_track_peak, has_album_gain, has_album_peak;
  gboolean has_ref_level;

  g_return_val_if_fail (event != NULL, NULL);
  g_return_val_if_fail (GST_EVENT_TYPE (event) == GST_EVENT_TAG, event);

  gst_event_parse_tag (event, &tag_list);

  if (gst_tag_list_is_empty (tag_list))
    return event;

  has_track_gain = gst_tag_list_get_double (tag_list, GST_TAG_TRACK_GAIN,
      &self->track_gain);
  has_track_peak = gst_tag_list_get_double (tag_list, GST_TAG_TRACK_PEAK,
      &self->track_peak);
  has_album_gain = gst_tag_list_get_double (tag_list, GST_TAG_ALBUM_GAIN,
      &self->album_gain);
  has_album_peak = gst_tag_list_get_double (tag_list, GST_TAG_ALBUM_PEAK,
      &self->album_peak);
  has_ref_level = gst_tag_list_get_double (tag_list, GST_TAG_REFERENCE_LEVEL,
      &self->reference_level);

  if (!has_track_gain && !has_track_peak && !has_album_gain && !has_album_peak)
    return event;

  if (has_ref_level && (has_track_gain || has_album_gain)
      && (ABS (self->reference_level - RG_REFERENCE_LEVEL) > 1.e-6)) {
    /* Log a message stating the amount of adjustment that is applied below. */
    GST_DEBUG_OBJECT (self,
        "compensating for reference level difference by %" GAIN_FORMAT,
        RG_REFERENCE_LEVEL - self->reference_level);
  }
  if (has_track_gain) {
    self->track_gain += RG_REFERENCE_LEVEL - self->reference_level;
  }
  if (has_album_gain) {
    self->album_gain += RG_REFERENCE_LEVEL - self->reference_level;
  }

  /* Ignore values that are obviously invalid. */
  if (G_UNLIKELY (has_track_gain && !VALID_GAIN (self->track_gain))) {
    GST_DEBUG_OBJECT (self,
        "ignoring bogus track gain value %" GAIN_FORMAT, self->track_gain);
    has_track_gain = FALSE;
  }
  if (G_UNLIKELY (has_track_peak && !VALID_PEAK (self->track_peak))) {
    GST_DEBUG_OBJECT (self,
        "ignoring bogus track peak value %" PEAK_FORMAT, self->track_peak);
    has_track_peak = FALSE;
  }
  if (G_UNLIKELY (has_album_gain && !VALID_GAIN (self->album_gain))) {
    GST_DEBUG_OBJECT (self,
        "ignoring bogus album gain value %" GAIN_FORMAT, self->album_gain);
    has_album_gain = FALSE;
  }
  if (G_UNLIKELY (has_album_peak && !VALID_PEAK (self->album_peak))) {
    GST_DEBUG_OBJECT (self,
        "ignoring bogus album peak value %" PEAK_FORMAT, self->album_peak);
    has_album_peak = FALSE;
  }

  /* Clamp peaks >1.0.  Float based decoders can produce spurious samples >1.0,
   * cutting these files back to 1.0 should not cause any audible distortion.
   * This is most often seen with Vorbis files. */
  if (has_track_peak && self->track_peak > 1.) {
    GST_DEBUG_OBJECT (self,
        "clamping track peak %" PEAK_FORMAT " to 1.0", self->track_peak);
    self->track_peak = 1.0;
  }
  if (has_album_peak && self->album_peak > 1.) {
    GST_DEBUG_OBJECT (self,
        "clamping album peak %" PEAK_FORMAT " to 1.0", self->album_peak);
    self->album_peak = 1.0;
  }

  self->has_track_gain |= has_track_gain;
  self->has_track_peak |= has_track_peak;
  self->has_album_gain |= has_album_gain;
  self->has_album_peak |= has_album_peak;

  event = (GstEvent *) gst_mini_object_make_writable (GST_MINI_OBJECT (event));
  gst_event_parse_tag (event, &tag_list);

  gst_tag_list_remove_tag (tag_list, GST_TAG_TRACK_GAIN);
  gst_tag_list_remove_tag (tag_list, GST_TAG_TRACK_PEAK);
  gst_tag_list_remove_tag (tag_list, GST_TAG_ALBUM_GAIN);
  gst_tag_list_remove_tag (tag_list, GST_TAG_ALBUM_PEAK);
  gst_tag_list_remove_tag (tag_list, GST_TAG_REFERENCE_LEVEL);

  gst_rg_volume_update_gain (self);

  if (gst_tag_list_is_empty (tag_list)) {
    gst_event_unref (event);
    event = NULL;
  }

  return event;
}
Esempio n. 21
0
GDALDataset *BTDataset::Open( GDALOpenInfo * poOpenInfo )

{
/* -------------------------------------------------------------------- */
/*      Verify that this is some form of binterr file.                  */
/* -------------------------------------------------------------------- */
    if( poOpenInfo->nHeaderBytes < 256)
        return NULL;

    if( strncmp( (const char *) poOpenInfo->pabyHeader, "binterr", 7 ) != 0 )
        return NULL;

/* -------------------------------------------------------------------- */
/*      Create the dataset.                                             */
/* -------------------------------------------------------------------- */
    BTDataset  *poDS;

    poDS = new BTDataset();
 
    memcpy( poDS->abyHeader, poOpenInfo->pabyHeader, 256 );

/* -------------------------------------------------------------------- */
/*      Get the version.                                                */
/* -------------------------------------------------------------------- */
    char szVersion[4];

    strncpy( szVersion, (char *) (poDS->abyHeader + 7), 3 );
    szVersion[3] = '\0';
    poDS->nVersionCode = (int) (atof(szVersion) * 10);

/* -------------------------------------------------------------------- */
/*      Extract core header information, being careful about the        */
/*      version.                                                        */
/* -------------------------------------------------------------------- */
    GInt32 nIntTemp;
    GInt16 nDataSize;
    GDALDataType eType;

    memcpy( &nIntTemp, poDS->abyHeader + 10, 4 );
    poDS->nRasterXSize = CPL_LSBWORD32( nIntTemp );
    
    memcpy( &nIntTemp, poDS->abyHeader + 14, 4 );
    poDS->nRasterYSize = CPL_LSBWORD32( nIntTemp );

    if (!GDALCheckDatasetDimensions(poDS->nRasterXSize, poDS->nRasterYSize))
    {
        delete poDS;
        return NULL;
    }

    memcpy( &nDataSize, poDS->abyHeader+18, 2 );
    nDataSize = CPL_LSBWORD16( nDataSize );

    if( poDS->abyHeader[20] != 0 && nDataSize == 4 )
        eType = GDT_Float32;
    else if( poDS->abyHeader[20] == 0 && nDataSize == 4 )
        eType = GDT_Int32;
    else if( poDS->abyHeader[20] == 0 && nDataSize == 2 )
        eType = GDT_Int16;
    else
    {
        CPLError( CE_Failure, CPLE_AppDefined, 
                  ".bt file data type unknown, got datasize=%d.", 
                  nDataSize );
        delete poDS;
        return NULL;
    }

    /*
        rcg, apr 7/06: read offset 62 for vert. units.
        If zero, assume 1.0 as per spec.

    */
    memcpy( &poDS->m_fVscale, poDS->abyHeader + 62, 4 );
    CPL_LSBPTR32(&poDS->m_fVscale);
    if(poDS->m_fVscale == 0.0f)
        poDS->m_fVscale = 1.0f; 

/* -------------------------------------------------------------------- */
/*      Try to read a .prj file if it is indicated.                     */
/* -------------------------------------------------------------------- */
    OGRSpatialReference oSRS;						

    if( poDS->nVersionCode >= 12 && poDS->abyHeader[60] != 0 )
    {
        const char  *pszPrjFile = CPLResetExtension( poOpenInfo->pszFilename, 
                                                     "prj" );
        VSILFILE *fp;

        fp = VSIFOpenL( pszPrjFile, "rt" );
        if( fp != NULL )
        {
            char *pszBuffer, *pszBufPtr;
            int  nBufMax = 10000;
            int nBytes;

            pszBuffer = (char *) CPLMalloc(nBufMax);
            nBytes = VSIFReadL( pszBuffer, 1, nBufMax-1, fp );
            VSIFCloseL( fp );

            pszBuffer[nBytes] = '\0';

            pszBufPtr = pszBuffer;
            if( oSRS.importFromWkt( &pszBufPtr ) != OGRERR_NONE )
            {
                CPLError( CE_Warning, CPLE_AppDefined, 
                          "Unable to parse .prj file, coordinate system missing." );
            }
            CPLFree( pszBuffer );
        }
    }

/* -------------------------------------------------------------------- */
/*      If we didn't find a .prj file, try to use internal info.        */
/* -------------------------------------------------------------------- */
    if( oSRS.GetRoot() == NULL )
    {
        GInt16 nUTMZone, nDatum, nHUnits;
        
        memcpy( &nUTMZone, poDS->abyHeader + 24, 2 );
        nUTMZone = CPL_LSBWORD16( nUTMZone );
        
        memcpy( &nDatum, poDS->abyHeader + 26, 2 );
        nDatum = CPL_LSBWORD16( nDatum );
        
        memcpy( &nHUnits, poDS->abyHeader + 22, 2 );
        nHUnits = CPL_LSBWORD16( nHUnits );

        if( nUTMZone != 0 )
            oSRS.SetUTM( ABS(nUTMZone), nUTMZone > 0 );
        else if( nHUnits != 0 )
            oSRS.SetLocalCS( "Unknown" );
        
        if( nHUnits == 1 )
            oSRS.SetLinearUnits( SRS_UL_METER, 1.0 );
        else if( nHUnits == 2 )
            oSRS.SetLinearUnits( SRS_UL_FOOT, atof(SRS_UL_FOOT_CONV) );
        else if( nHUnits == 3 )
            oSRS.SetLinearUnits( SRS_UL_US_FOOT, atof(SRS_UL_US_FOOT_CONV) );

        // Translate some of the more obvious old USGS datum codes 
        if( nDatum == 0 )
            nDatum = 6201;
        else if( nDatum == 1 )
            nDatum = 6209;
        else if( nDatum == 2 )
            nDatum = 6210;
        else if( nDatum == 3 )
            nDatum = 6202;
        else if( nDatum == 4 )
            nDatum = 6203;
        else if( nDatum == 4 )
            nDatum = 6203;
        else if( nDatum == 6 )
            nDatum = 6222;
        else if( nDatum == 7 )
            nDatum = 6230;
        else if( nDatum == 13 )
            nDatum = 6267;
        else if( nDatum == 14 )
            nDatum = 6269;
        else if( nDatum == 17 )
            nDatum = 6277;
        else if( nDatum == 19 )
            nDatum = 6284;
        else if( nDatum == 21 )
            nDatum = 6301;
        else if( nDatum == 22 )
            nDatum = 6322;
        else if( nDatum == 23 )
            nDatum = 6326;

        if( !oSRS.IsLocal() )
        {
            if( nDatum >= 6000 )
            {
                char szName[32];
                sprintf( szName, "EPSG:%d", nDatum-2000 );
                oSRS.SetWellKnownGeogCS( szName );
            }
            else
                oSRS.SetWellKnownGeogCS( "WGS84" );
        }                
    }

/* -------------------------------------------------------------------- */
/*      Convert coordinate system back to WKT.                          */
/* -------------------------------------------------------------------- */
    if( oSRS.GetRoot() != NULL )
        oSRS.exportToWkt( &poDS->pszProjection );

/* -------------------------------------------------------------------- */
/*      Get georeferencing bounds.                                      */
/* -------------------------------------------------------------------- */
    if( poDS->nVersionCode >= 11 )
    {
        double dfLeft, dfRight, dfTop, dfBottom;

        memcpy( &dfLeft, poDS->abyHeader + 28, 8 );
        CPL_LSBPTR64( &dfLeft );
        
        memcpy( &dfRight, poDS->abyHeader + 36, 8 );
        CPL_LSBPTR64( &dfRight );
        
        memcpy( &dfBottom, poDS->abyHeader + 44, 8 );
        CPL_LSBPTR64( &dfBottom );
        
        memcpy( &dfTop, poDS->abyHeader + 52, 8 );
        CPL_LSBPTR64( &dfTop );

        poDS->adfGeoTransform[0] = dfLeft;
        poDS->adfGeoTransform[1] = (dfRight - dfLeft) / poDS->nRasterXSize;
        poDS->adfGeoTransform[2] = 0.0;
        poDS->adfGeoTransform[3] = dfTop;
        poDS->adfGeoTransform[4] = 0.0;
        poDS->adfGeoTransform[5] = (dfBottom - dfTop) / poDS->nRasterYSize;
        
        poDS->bGeoTransformValid = TRUE;
    }
    
/* -------------------------------------------------------------------- */
/*      Re-open the file with the desired access.                       */
/* -------------------------------------------------------------------- */

    if( poOpenInfo->eAccess == GA_Update )
        poDS->fpImage = VSIFOpenL( poOpenInfo->pszFilename, "rb+" );
    else
        poDS->fpImage = VSIFOpenL( poOpenInfo->pszFilename, "rb" );

    if( poDS->fpImage == NULL )
    {
        CPLError( CE_Failure, CPLE_OpenFailed,
                  "Failed to re-open %s within BT driver.\n",
                  poOpenInfo->pszFilename );
        return NULL;
    }
    poDS->eAccess = poOpenInfo->eAccess;

/* -------------------------------------------------------------------- */
/*      Create band information objects                                 */
/* -------------------------------------------------------------------- */
    poDS->SetBand( 1, new BTRasterBand( poDS, poDS->fpImage, eType ) );

#ifdef notdef
    poDS->bGeoTransformValid = 
        GDALReadWorldFile( poOpenInfo->pszFilename, ".wld", 
                           poDS->adfGeoTransform );
#endif

/* -------------------------------------------------------------------- */
/*      Initialize any PAM information.                                 */
/* -------------------------------------------------------------------- */
    poDS->SetDescription( poOpenInfo->pszFilename );
    poDS->TryLoadXML();

/* -------------------------------------------------------------------- */
/*      Check for overviews.                                            */
/* -------------------------------------------------------------------- */
    poDS->oOvManager.Initialize( poDS, poOpenInfo->pszFilename );

    return( poDS );
}
Esempio n. 22
0
int main(int argc, char ** argv) {
 
  int    Num_procs;       /* number of ranks                                     */
  int    Num_procsx, 
         Num_procsy;      /* number of ranks in each coord direction             */
  int    Num_groupsx, 
         Num_groupsy;     /* number of blocks in each coord direction            */
  int    my_group;        /* sequence number of shared memory block              */
  int    my_group_IDx,
         my_group_IDy;    /* coordinates of block within block grid              */
  int    group_size;      /* number of ranks in shared memory group              */
  int    group_sizex,
         group_sizey;     /* number of ranks in block in each coord direction    */
  int    my_ID;           /* MPI rank                                            */
  int    my_global_IDx, 
         my_global_IDy;   /* coordinates of rank in overall rank grid            */
  int    my_local_IDx, 
         my_local_IDy;    /* coordinates of rank within shared memory block      */
  int    right_nbr;       /* global rank of right neighboring tile               */
  int    left_nbr;        /* global rank of left neighboring tile                */
  int    top_nbr;         /* global rank of top neighboring tile                 */
  int    bottom_nbr;      /* global rank of bottom neighboring tile              */
  int    local_nbr[4];    /* list of synchronizing local neighbors               */
  int    num_local_nbrs;  /* number of synchronizing local neighbors             */
  int    dummy;
  DTYPE *top_buf_out;     /* communication buffer                                */
  DTYPE *top_buf_in;      /*       "         "                                   */
  DTYPE *bottom_buf_out;  /*       "         "                                   */
  DTYPE *bottom_buf_in;   /*       "         "                                   */
  DTYPE *right_buf_out;   /*       "         "                                   */
  DTYPE *right_buf_in;    /*       "         "                                   */
  DTYPE *left_buf_out;    /*       "         "                                   */
  DTYPE *left_buf_in;     /*       "         "                                   */
  int    root = 0;
  long   n, width, height;/* linear global and block grid dimension              */
  int    width_rank, 
         height_rank;     /* linear local dimension                              */
  int    i, j, ii, jj, kk, it, jt, iter, leftover;  /* dummies                   */
  int    istart_rank, 
         iend_rank;       /* bounds of grid tile assigned to calling rank        */
  int    jstart_rank, 
         jend_rank;       /* bounds of grid tile assigned to calling rank        */
  int    istart, iend;    /* bounds of grid block containing tile                */
  int    jstart, jend;    /* bounds of grid block containing tile                */
  DTYPE  norm,            /* L1 norm of solution                                 */
         local_norm,      /* contribution of calling rank to L1 norm             */
         reference_norm;  /* value to be matched by computed norm                */
  DTYPE  f_active_points; /* interior of grid with respect to stencil            */
  DTYPE  flops;           /* floating point ops per iteration                    */
  int    iterations;      /* number of times to run the algorithm                */
  double local_stencil_time,/* timing parameters                                 */
         stencil_time,
         avgtime; 
  int    stencil_size;    /* number of points in stencil                         */
  DTYPE  * RESTRICT in;   /* input grid values                                   */
  DTYPE  * RESTRICT out;  /* output grid values                                  */
  long   total_length_in; /* total required length to store input array          */
  long   total_length_out;/* total required length to store output array         */
  int    error=0;         /* error flag                                          */
  DTYPE  weight[2*RADIUS+1][2*RADIUS+1]; /* weights of points in the stencil     */
  MPI_Request request[8]; /* requests for sends & receives in 4 coord directions */
  MPI_Status  status[8];  /* corresponding statuses                              */
  MPI_Win shm_win_in;     /* shared memory window object for IN array            */
  MPI_Win shm_win_out;    /* shared memory window object for OUT array           */
  MPI_Comm shm_comm_prep; /* preparatory shared memory communicator              */
  MPI_Comm shm_comm;      /* Shared Memory Communicator                          */
  int shm_procs;          /* # of rankes in shared domain                        */
  int shm_ID;             /* MPI rank in shared memory domain                    */
  MPI_Aint size_in;       /* size of the IN array in shared memory window        */
  MPI_Aint size_out;      /* size of the OUT array in shared memory window       */
  int size_mul;           /* one for shm_comm root, zero for the other ranks     */
  int disp_unit;          /* ignored                                             */
 
  /*******************************************************************************
  ** Initialize the MPI environment
  ********************************************************************************/
  MPI_Init(&argc,&argv);
  MPI_Comm_rank(MPI_COMM_WORLD, &my_ID);
  MPI_Comm_size(MPI_COMM_WORLD, &Num_procs);
 
  /*******************************************************************************
  ** process, test, and broadcast input parameters    
  ********************************************************************************/
 
  if (my_ID == root) {
    printf("Parallel Research Kernels version %s\n", PRKVERSION);
    printf("MPI+SHM stencil execution on 2D grid\n");

#ifndef STAR
      printf("ERROR: Compact stencil not supported\n");
      error = 1;
      goto ENDOFTESTS;
#endif
    
    if (argc != 4){
      printf("Usage: %s  <#ranks per coherence domain><# iterations> <array dimension> \n", 
             *argv);
      error = 1;
      goto ENDOFTESTS;
    }
 
    group_size = atoi(*++argv);
    if (group_size < 1) {
      printf("ERROR: # ranks per coherence domain must be >= 1 : %d \n",group_size);
      error = 1;
      goto ENDOFTESTS;
    } 
    if (Num_procs%group_size) {
      printf("ERROR: total # %d ranks not divisible by ranks per coherence domain %d\n",
	     Num_procs, group_size);
      error = 1;
      goto ENDOFTESTS;
    } 

    iterations  = atoi(*++argv); 
    if (iterations < 0){
      printf("ERROR: iterations must be >= 0 : %d \n",iterations);
      error = 1;
      goto ENDOFTESTS;  
    }
 
    n  = atol(*++argv);
    long nsquare = n * n;
    if (nsquare < Num_procs){ 
      printf("ERROR: grid size must be at least # ranks: %ld\n", nsquare);
      error = 1;
      goto ENDOFTESTS;
    }
 
    if (RADIUS < 0) {
      printf("ERROR: Stencil radius %d should be non-negative\n", RADIUS);
      error = 1;
      goto ENDOFTESTS;  
    }
 
    if (2*RADIUS +1 > n) {
      printf("ERROR: Stencil radius %d exceeds grid size %ld\n", RADIUS, n);
      error = 1;
      goto ENDOFTESTS;  
    }
 
    ENDOFTESTS:;  
  }
  bail_out(error);

  MPI_Bcast(&n,          1, MPI_LONG, root, MPI_COMM_WORLD);
  MPI_Bcast(&iterations, 1, MPI_INT, root, MPI_COMM_WORLD);
  MPI_Bcast(&group_size, 1, MPI_INT, root, MPI_COMM_WORLD);
 
  /* determine best way to create a 2D grid of ranks (closest to square, for 
     best surface/volume ratio); we do this brute force for now. The 
     decomposition needs to be such that shared memory groups can evenly
     tessellate the rank grid
  */
  for (Num_procsx=(int) (sqrt(Num_procs+1)); Num_procsx>0; Num_procsx--) {
    if (!(Num_procs%Num_procsx)) {
      Num_procsy = Num_procs/Num_procsx;
      for (group_sizex=(int)(sqrt(group_size+1)); group_sizex>0; group_sizex--) {
        if (!(group_size%group_sizex) && !(Num_procsx%group_sizex)) {
          group_sizey=group_size/group_sizex;
          break;
        }
      }
      if (!(Num_procsy%group_sizey)) break;
    }
  }      


  if (my_ID == root) {
    printf("Number of ranks                 = %d\n", Num_procs);
    printf("Grid size                       = %ld\n", n);
    printf("Radius of stencil               = %d\n", RADIUS);
    printf("Tiles in x/y-direction          = %d/%d\n", Num_procsx, Num_procsy);
    printf("Tiles per shared memory domain  = %d\n", group_size);
    printf("Tiles in x/y-direction in group = %d/%d\n", group_sizex,  group_sizey);
    printf("Type of stencil                 = star\n");
#ifdef LOCAL_BARRIER_SYNCH
    printf("Local synchronization           = barrier\n");
#else
    printf("Local synchronization           = point to point\n");
#endif
#ifdef DOUBLE
    printf("Data type                       = double precision\n");
#else
    printf("Data type                       = single precision\n");
#endif
#if LOOPGEN
    printf("Script used to expand stencil loop body\n");
#else
    printf("Compact representation of stencil loop body\n");
#endif
    printf("Number of iterations            = %d\n", iterations);
  }

  /* Setup for Shared memory regions */

  /* first divide WORLD in groups of size group_size */
  MPI_Comm_split(MPI_COMM_WORLD, my_ID/group_size, my_ID%group_size, &shm_comm_prep);
  /* derive from that an SHM communicator */
  MPI_Comm_split_type(shm_comm_prep, MPI_COMM_TYPE_SHARED, 0, MPI_INFO_NULL, &shm_comm);
  MPI_Comm_rank(shm_comm, &shm_ID);
  MPI_Comm_size(shm_comm, &shm_procs);
  /* do sanity check, making sure groups did not shrink in second comm split */
  if (shm_procs != group_size) MPI_Abort(MPI_COMM_WORLD, 666);
  
  Num_groupsx = Num_procsx/group_sizex;
  Num_groupsy = Num_procsy/group_sizey;

  my_group = my_ID/group_size;
  my_group_IDx = my_group%Num_groupsx;
  my_group_IDy = my_group/Num_groupsx;
  my_local_IDx = my_ID%group_sizex;
  my_local_IDy = (my_ID%group_size)/group_sizex;
  my_global_IDx = my_group_IDx*group_sizex+my_local_IDx;
  my_global_IDy = my_group_IDy*group_sizey+my_local_IDy;

  /* set all neighboring ranks to -1 (no communication with those ranks) */
  left_nbr = right_nbr = top_nbr = bottom_nbr = -1;
  /* keep track of local neighbors for local synchronization             */
  num_local_nbrs = 0;

  if (my_local_IDx == group_sizex-1 && my_group_IDx != (Num_groupsx-1)) {
    right_nbr = (my_group+1)*group_size+shm_ID-group_sizex+1;
  }
  if (my_local_IDx != group_sizex-1) {
    local_nbr[num_local_nbrs++] = shm_ID + 1;
  }

  if (my_local_IDx == 0 && my_group_IDx != 0) {
    left_nbr = (my_group-1)*group_size+shm_ID+group_sizex-1;
  }
  if (my_local_IDx != 0) {
    local_nbr[num_local_nbrs++] = shm_ID - 1;
  }

  if (my_local_IDy == group_sizey-1 && my_group_IDy != (Num_groupsy-1)) {
    top_nbr = (my_group+Num_groupsx)*group_size + my_local_IDx;
  }
  if (my_local_IDy != group_sizey-1) {
    local_nbr[num_local_nbrs++] = shm_ID + group_sizex;
  }

  if (my_local_IDy == 0 && my_group_IDy != 0) {
    bottom_nbr = (my_group-Num_groupsx)*group_size + group_sizex*(group_sizey-1)+my_local_IDx;
  }
  if (my_local_IDy != 0) {
    local_nbr[num_local_nbrs++] = shm_ID - group_sizex;
  }

  /* compute amount of space required for input and solution arrays for the block,
     and also compute index sets                                                  */
  
  width = n/Num_groupsx;
  leftover = n%Num_groupsx;
  if (my_group_IDx<leftover) {
    istart = (width+1) * my_group_IDx; 
    iend = istart + width;
  }
  else {
    istart = (width+1) * leftover + width * (my_group_IDx-leftover);
    iend = istart + width - 1;
  }
  
  width = iend - istart + 1;
  if (width == 0) {
    printf("ERROR: rank %d has no work to do\n", my_ID);
    error = 1;
  }
  bail_out(error);
 
  height = n/Num_groupsy;
  leftover = n%Num_groupsy;
  if (my_group_IDy<leftover) {
    jstart = (height+1) * my_group_IDy; 
    jend = jstart + height;
  }
  else {
    jstart = (height+1) * leftover + height * (my_group_IDy-leftover);
    jend = jstart + height - 1;
  }
  
  height = jend - jstart + 1;
  if (height == 0) {
    printf("ERROR: rank %d has no work to do\n", my_ID);
    error = 1;
  }
  bail_out(error);
 
  if (width < RADIUS || height < RADIUS) {
    printf("ERROR: rank %d has work tile smaller then stencil radius; w=%ld,h=%ld\n",
           my_ID, width, height);
    error = 1;
  }
  bail_out(error);
 
  total_length_in = (width+2*RADIUS)*(height+2*RADIUS)*sizeof(DTYPE);
  total_length_out = width*height*sizeof(DTYPE);

  /* only the root of each SHM domain specifies window of nonzero size */
  size_mul = (shm_ID==0);  
  size_in= total_length_in*size_mul; 
  MPI_Win_allocate_shared(size_in, sizeof(double), MPI_INFO_NULL, shm_comm, 
                          (void *) &in, &shm_win_in);
  MPI_Win_lock_all(MPI_MODE_NOCHECK, shm_win_in);
  MPI_Win_shared_query(shm_win_in, MPI_PROC_NULL, &size_in, &disp_unit, (void *)&in);
  if (in == NULL){
    printf("Error allocating space for input array by group %d\n",my_group);
    error = 1;
  }
  bail_out(error);

  size_out= total_length_out*size_mul;
  MPI_Win_allocate_shared(size_out, sizeof(double), MPI_INFO_NULL, shm_comm, 
                          (void *) &out, &shm_win_out);
  MPI_Win_lock_all(MPI_MODE_NOCHECK, shm_win_out);
  MPI_Win_shared_query(shm_win_out, MPI_PROC_NULL, &size_out, &disp_unit, (void *)&out);
  if (out == NULL){
    printf("Error allocating space for output array by group %d\n", my_group);
    error = 1;
  }
  bail_out(error);

  /* determine index set assigned to each rank                         */

  width_rank = width/group_sizex;
  leftover = width%group_sizex;
  if (my_local_IDx<leftover) {
    istart_rank = (width_rank+1) * my_local_IDx; 
    iend_rank = istart_rank + width_rank;
  }
  else {
    istart_rank = (width_rank+1) * leftover + width_rank * (my_local_IDx-leftover);
    iend_rank = istart_rank + width_rank - 1;
  }
  istart_rank += istart;
  iend_rank += istart;
  width_rank = iend_rank - istart_rank + 1;   

  height_rank = height/group_sizey;
  leftover = height%group_sizey;
  if (my_local_IDy<leftover) {
    jstart_rank = (height_rank+1) * my_local_IDy; 
    jend_rank = jstart_rank + height_rank;
  }
  else {
    jstart_rank = (height_rank+1) * leftover + height_rank * (my_local_IDy-leftover);
    jend_rank = jstart_rank + height_rank - 1;
  }
  jstart_rank+=jstart;
  jend_rank+=jstart;
  height_rank = jend_rank - jstart_rank + 1;

  if (height_rank*width_rank==0) {
    error = 1;
    printf("Rank %d has no work to do\n", my_ID);
  }
  bail_out(error);

  /* allocate communication buffers for halo values                            */
  top_buf_out = (DTYPE *) malloc(4*sizeof(DTYPE)*RADIUS*width_rank);
  if (!top_buf_out) {
    printf("ERROR: Rank %d could not allocated comm buffers for y-direction\n", my_ID);
    error = 1;
  }
  bail_out(error);
  top_buf_in     = top_buf_out +   RADIUS*width_rank;
  bottom_buf_out = top_buf_out + 2*RADIUS*width_rank;
  bottom_buf_in  = top_buf_out + 3*RADIUS*width_rank;
 
  right_buf_out = (DTYPE *) malloc(4*sizeof(DTYPE)*RADIUS*height_rank);
  if (!right_buf_out) { 
    printf("ERROR: Rank %d could not allocated comm buffers for x-direction\n", my_ID);
    error = 1;
  }
  bail_out(error);
  right_buf_in   = right_buf_out +   RADIUS*height_rank;
  left_buf_out   = right_buf_out + 2*RADIUS*height_rank;
  left_buf_in    = right_buf_out + 3*RADIUS*height_rank;

    /* fill the stencil weights to reflect a discrete divergence operator         */
  for (jj=-RADIUS; jj<=RADIUS; jj++) for (ii=-RADIUS; ii<=RADIUS; ii++)
    WEIGHT(ii,jj) = (DTYPE) 0.0;
  stencil_size = 4*RADIUS+1;
  for (ii=1; ii<=RADIUS; ii++) {
    WEIGHT(0, ii) = WEIGHT( ii,0) =  (DTYPE) (1.0/(2.0*ii*RADIUS));
    WEIGHT(0,-ii) = WEIGHT(-ii,0) = -(DTYPE) (1.0/(2.0*ii*RADIUS));
  }

  norm = (DTYPE) 0.0;
  f_active_points = (DTYPE) (n-2*RADIUS)*(DTYPE) (n-2*RADIUS);
  /* intialize the input and output arrays                                     */
  for (j=jstart_rank; j<=jend_rank; j++) for (i=istart_rank; i<=iend_rank; i++) {
    IN(i,j)  = COEFX*i+COEFY*j;
    OUT(i,j) = (DTYPE)0.0;
  }

  /* LOAD/STORE FENCE */
  MPI_Win_sync(shm_win_in);
  MPI_Win_sync(shm_win_out);
  MPI_Barrier(shm_comm); 

  for (iter = 0; iter<=iterations; iter++){

    /* start timer after a warmup iteration */
    if (iter == 1) { 
      MPI_Barrier(MPI_COMM_WORLD);
      local_stencil_time = wtime();
    }

    /* need to fetch ghost point data from neighbors in y-direction                 */
    if (top_nbr != -1) {
      MPI_Irecv(top_buf_in, RADIUS*width_rank, MPI_DTYPE, top_nbr, 101,
                MPI_COMM_WORLD, &(request[1]));
      for (kk=0,j=jend_rank-RADIUS+1; j<=jend_rank; j++) 
      for (i=istart_rank; i<=iend_rank; i++) {
        top_buf_out[kk++]= IN(i,j);
      }
      MPI_Isend(top_buf_out, RADIUS*width_rank,MPI_DTYPE, top_nbr, 99, 
                MPI_COMM_WORLD, &(request[0]));
    }

    if (bottom_nbr != -1) {
      MPI_Irecv(bottom_buf_in,RADIUS*width_rank, MPI_DTYPE, bottom_nbr, 99, 
                MPI_COMM_WORLD, &(request[3]));
      for (kk=0,j=jstart_rank; j<=jstart_rank+RADIUS-1; j++) 
      for (i=istart_rank; i<=iend_rank; i++) {
        bottom_buf_out[kk++]= IN(i,j);
      }
      MPI_Isend(bottom_buf_out, RADIUS*width_rank,MPI_DTYPE, bottom_nbr, 101,
 	  MPI_COMM_WORLD, &(request[2]));
      }

    if (top_nbr != -1) {
      MPI_Wait(&(request[0]), &(status[0]));
      MPI_Wait(&(request[1]), &(status[1]));
      for (kk=0,j=jend_rank+1; j<=jend_rank+RADIUS; j++) 
      for (i=istart_rank; i<=iend_rank; i++) {
        IN(i,j) = top_buf_in[kk++];
      }
    }

    if (bottom_nbr != -1) {    
      MPI_Wait(&(request[2]), &(status[2]));
      MPI_Wait(&(request[3]), &(status[3]));
      for (kk=0,j=jstart_rank-RADIUS; j<=jstart_rank-1; j++) 
      for (i=istart_rank; i<=iend_rank; i++) {
        IN(i,j) = bottom_buf_in[kk++];
      }
    }

    /* LOAD/STORE FENCE */
    MPI_Win_sync(shm_win_in);

    /* need to fetch ghost point data from neighbors in x-direction                 */
    if (right_nbr != -1) {
      MPI_Irecv(right_buf_in, RADIUS*height_rank, MPI_DTYPE, right_nbr, 1010,
                MPI_COMM_WORLD, &(request[1+4]));
      for (kk=0,j=jstart_rank; j<=jend_rank; j++) 
      for (i=iend_rank-RADIUS+1; i<=iend_rank; i++) {
        right_buf_out[kk++]= IN(i,j);
      }
      MPI_Isend(right_buf_out, RADIUS*height_rank, MPI_DTYPE, right_nbr, 990, 
                MPI_COMM_WORLD, &(request[0+4]));
    }

    if (left_nbr != -1) {
      MPI_Irecv(left_buf_in, RADIUS*height_rank, MPI_DTYPE, left_nbr, 990, 
                MPI_COMM_WORLD, &(request[3+4]));
      for (kk=0,j=jstart_rank; j<=jend_rank; j++) 
      for (i=istart_rank; i<=istart_rank+RADIUS-1; i++) {
        left_buf_out[kk++]= IN(i,j);
      }
      MPI_Isend(left_buf_out, RADIUS*height_rank, MPI_DTYPE, left_nbr, 1010,
                MPI_COMM_WORLD, &(request[2+4]));
    }

    if (right_nbr != -1) {
      MPI_Wait(&(request[0+4]), &(status[0+4]));
      MPI_Wait(&(request[1+4]), &(status[1+4]));
      for (kk=0,j=jstart_rank; j<=jend_rank; j++) 
      for (i=iend_rank+1; i<=iend_rank+RADIUS; i++) {
        IN(i,j) = right_buf_in[kk++];
      }
    }

    if (left_nbr != -1) {
      MPI_Wait(&(request[2+4]), &(status[2+4]));
      MPI_Wait(&(request[3+4]), &(status[3+4]));
      for (kk=0,j=jstart_rank; j<=jend_rank; j++) 
      for (i=istart_rank-RADIUS; i<=istart_rank-1; i++) {
        IN(i,j) = left_buf_in[kk++];
      }
    }

    /* LOAD/STORE FENCE */
    MPI_Win_sync(shm_win_in);

    /* Apply the stencil operator */
    for (j=MAX(jstart_rank,RADIUS); j<=MIN(n-RADIUS-1,jend_rank); j++) {
      for (i=MAX(istart_rank,RADIUS); i<=MIN(n-RADIUS-1,iend_rank); i++) {
        #if LOOPGEN
          #include "loop_body_star.incl"
        #else
          for (jj=-RADIUS; jj<=RADIUS; jj++) OUT(i,j) += WEIGHT(0,jj)*IN(i,j+jj);
          for (ii=-RADIUS; ii<0; ii++)       OUT(i,j) += WEIGHT(ii,0)*IN(i+ii,j);
          for (ii=1; ii<=RADIUS; ii++)       OUT(i,j) += WEIGHT(ii,0)*IN(i+ii,j);
        #endif
      }
    }

    /* LOAD/STORE FENCE */
    MPI_Win_sync(shm_win_out);

#ifdef LOCAL_BARRIER_SYNCH
    MPI_Barrier(shm_comm); // needed to avoid writing IN while other ranks are reading it
#else
    for (i=0; i<num_local_nbrs; i++) {
      MPI_Irecv(&dummy, 0, MPI_INT, local_nbr[i], 666, shm_comm, &(request[i]));
      MPI_Send(&dummy, 0, MPI_INT, local_nbr[i], 666, shm_comm);
    }
    MPI_Waitall(num_local_nbrs, request, status);
#endif

    /* add constant to solution to force refresh of neighbor data, if any */
    for (j=jstart_rank; j<=jend_rank; j++) 
    for (i=istart_rank; i<=iend_rank; i++) IN(i,j)+= 1.0;

    /* LOAD/STORE FENCE */
    MPI_Win_sync(shm_win_in);

#ifdef LOCAL_BARRIER_SYNCH
    MPI_Barrier(shm_comm); // needed to avoid reading IN while other ranks are writing it
#else
    for (i=0; i<num_local_nbrs; i++) {
      MPI_Irecv(&dummy, 0, MPI_INT, local_nbr[i], 666, shm_comm, &(request[i]));
      MPI_Send(&dummy, 0, MPI_INT, local_nbr[i], 666, shm_comm);
    }
    MPI_Waitall(num_local_nbrs, request, status);
#endif
 
  } /* end of iterations                                                   */
 
  local_stencil_time = wtime() - local_stencil_time;
  MPI_Reduce(&local_stencil_time, &stencil_time, 1, MPI_DOUBLE, MPI_MAX, root,
             MPI_COMM_WORLD);
  
  /* compute L1 norm in parallel                                                */
  local_norm = (DTYPE) 0.0;
  for (j=MAX(jstart_rank,RADIUS); j<=MIN(n-RADIUS-1,jend_rank); j++) {
    for (i=MAX(istart_rank,RADIUS); i<=MIN(n-RADIUS-1,iend_rank); i++) {
      local_norm += (DTYPE)ABS(OUT(i,j));
    }
  }
 
  MPI_Reduce(&local_norm, &norm, 1, MPI_DTYPE, MPI_SUM, root, MPI_COMM_WORLD);
 
  /*******************************************************************************
  ** Analyze and output results.
  ********************************************************************************/
 
/* verify correctness                                                            */
  if (my_ID == root) {
    norm /= f_active_points;
    if (RADIUS > 0) {
      reference_norm = (DTYPE) (iterations+1) * (COEFX + COEFY);
    }
    else {
      reference_norm = (DTYPE) 0.0;
    }
    if (ABS(norm-reference_norm) > EPSILON) {
      printf("ERROR: L1 norm = "FSTR", Reference L1 norm = "FSTR"\n",
             norm, reference_norm);
      error = 1;
    }
    else {
      printf("Solution validates\n");
#ifdef VERBOSE
      printf("Reference L1 norm = "FSTR", L1 norm = "FSTR"\n", 
             reference_norm, norm);
#endif
    }
  }
  bail_out(error);
 
  MPI_Win_unlock_all(shm_win_in);
  MPI_Win_unlock_all(shm_win_out);
  MPI_Win_free(&shm_win_in);
  MPI_Win_free(&shm_win_out);

  if (my_ID == root) {
    /* flops/stencil: 2 flops (fma) for each point in the stencil, 
       plus one flop for the update of the input of the array        */
    flops = (DTYPE) (2*stencil_size+1) * f_active_points;
    avgtime = stencil_time/iterations;
    printf("Rate (MFlops/s): "FSTR"  Avg time (s): %lf\n",
           1.0E-06 * flops/avgtime, avgtime);
  }
 
  MPI_Finalize();
  exit(EXIT_SUCCESS);
}
Esempio n. 23
0
/*
This function processes RX dependent coefficients when new RX commands are available
Those are: TPA, throttle expo
*/
void processRxDependentCoefficients(void) {

    int32_t tmp, tmp2;
    int32_t axis, prop1 = 0, prop2;

    // PITCH & ROLL only dynamic PID adjustment,  depending on throttle value
    if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
        prop2 = 100;
    } else {
        if (rcData[THROTTLE] < 2000) {
            prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
        } else {
            prop2 = 100 - currentControlRateProfile->dynThrPID;
        }
    }

    for (axis = 0; axis < 3; axis++) {
        tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
        if (axis == ROLL || axis == PITCH) {
            if (rcControlsConfig()->deadband) {
                if (tmp > rcControlsConfig()->deadband) {
                    tmp -= rcControlsConfig()->deadband;
                } else {
                    tmp = 0;
                }
            }

            tmp2 = tmp / 100;
            rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
            prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500;
            prop1 = (uint16_t)prop1 * prop2 / 100;
        } else if (axis == YAW) {
            if (rcControlsConfig()->yaw_deadband) {
                if (tmp > rcControlsConfig()->yaw_deadband) {
                    tmp -= rcControlsConfig()->yaw_deadband;
                } else {
                    tmp = 0;
                }
            }
            tmp2 = tmp / 100;
            rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -rcControlsConfig()->yaw_control_direction;
            prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500;
        }
#ifndef SKIP_PID_MW23
        // FIXME axis indexes into pids.  use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
        dynP8[axis] = (uint16_t)pidProfile()->P8[axis] * prop1 / 100;
        dynI8[axis] = (uint16_t)pidProfile()->I8[axis] * prop1 / 100;
        dynD8[axis] = (uint16_t)pidProfile()->D8[axis] * prop1 / 100;
#endif
        // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids
        if (axis == YAW) {
            PIDweight[axis] = 100;
        }
        else {
            PIDweight[axis] = prop2;
        }

        if (rcData[axis] < rxConfig()->midrc)
            rcCommand[axis] = -rcCommand[axis];
    }
    if (FLIGHT_MODE(HEADFREE_MODE)) {
        float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
        float cosDiff = cos_approx(radDiff);
        float sinDiff = sin_approx(radDiff);
        int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
        rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
        rcCommand[PITCH] = rcCommand_PITCH;
    }
}
Esempio n. 24
0
void Functions::fabs(Aurora::NWScript::FunctionContext &ctx) {
	ctx.getReturn() = ABS(ctx.getParams()[0].getFloat());
}
Esempio n. 25
0
void ToltecsEngine::walk(byte *walkData) {
	int16 xdelta, ydelta, v8, v10, v11;
	int16 xstep, ystep;
	ScriptWalk walkInfo;

	walkInfo.y = READ_LE_UINT16(walkData + 0);
	walkInfo.x = READ_LE_UINT16(walkData + 2);
	walkInfo.y1 = READ_LE_UINT16(walkData + 4);
	walkInfo.x1 = READ_LE_UINT16(walkData + 6);
	walkInfo.y2 = READ_LE_UINT16(walkData + 8);
	walkInfo.x2 = READ_LE_UINT16(walkData + 10);
	walkInfo.yerror = READ_LE_UINT16(walkData + 12);
	walkInfo.xerror = READ_LE_UINT16(walkData + 14);
	walkInfo.mulValue = READ_LE_UINT16(walkData + 16);
	walkInfo.scaling = READ_LE_UINT16(walkData + 18);

	walkInfo.scaling = -_segmap->getScalingAtPoint(walkInfo.x, walkInfo.y);

	if (walkInfo.y1 < walkInfo.y2)
		ystep = -1;
	else
		ystep = 1;
	ydelta = ABS(walkInfo.y1 - walkInfo.y2) * _walkSpeedY;

	if (walkInfo.x1 < walkInfo.x2)
		xstep = -1;
	else
		xstep = 1;
	xdelta = ABS(walkInfo.x1 - walkInfo.x2) * _walkSpeedX;

	debug(0, "ToltecsEngine::walk() xdelta = %d; ydelta = %d", xdelta, ydelta);

	if (xdelta > ydelta)
		SWAP(xdelta, ydelta);

	v8 = 100 * xdelta;
	if (v8 != 0) {
		if (walkInfo.scaling > 0)
			v8 -= v8 * ABS(walkInfo.scaling) / 100;
		else
			v8 += v8 * ABS(walkInfo.scaling) / 100;
		if (ydelta != 0)
			v8 /= ydelta;
	}

	if (ydelta > ABS(walkInfo.x1 - walkInfo.x2) * _walkSpeedX) {
		v10 = 100 - walkInfo.scaling;
		v11 = v8;
  	} else {
		v10 = v8;
  		v11 = 100 - walkInfo.scaling;
	}

	walkInfo.yerror += walkInfo.mulValue * v10;
	while (walkInfo.yerror >= 100 * _walkSpeedY) {
		walkInfo.yerror -= 100 * _walkSpeedY;
		if (walkInfo.y == walkInfo.y1) {
			walkInfo.x = walkInfo.x1;
			break;
		}
		walkInfo.y += ystep;
	}

	walkInfo.xerror += walkInfo.mulValue * v11;
	while (walkInfo.xerror >= 100 * _walkSpeedX) {
		walkInfo.xerror -= 100 * _walkSpeedX;
		if (walkInfo.x == walkInfo.x1) {
			walkInfo.y = walkInfo.y1;
			break;
		}
		walkInfo.x += xstep;
	}

	WRITE_LE_UINT16(walkData + 0, walkInfo.y);
	WRITE_LE_UINT16(walkData + 2, walkInfo.x);
	WRITE_LE_UINT16(walkData + 4, walkInfo.y1);
	WRITE_LE_UINT16(walkData + 6, walkInfo.x1);
	WRITE_LE_UINT16(walkData + 8, walkInfo.y2);
	WRITE_LE_UINT16(walkData + 10, walkInfo.x2);
	WRITE_LE_UINT16(walkData + 12, walkInfo.yerror);
	WRITE_LE_UINT16(walkData + 14, walkInfo.xerror);
	WRITE_LE_UINT16(walkData + 16, walkInfo.mulValue);
	WRITE_LE_UINT16(walkData + 18, walkInfo.scaling);
}
Esempio n. 26
0
/*
    offsetIndex:    [0] skeleton
                    [1] spline
                    [2] offset spline
*/
ofVec2f BGGraphics::calculateInternalTexOffset(float t, bool isSourceSpline, bool isSourceSegment, int offsetIndex) {

    const float triangleHeight = .5 * tanf(M_PI / 3.0);

    const float baseSize = sqrtf(3.0);
    const float halfBaseSize = .5 * baseSize;
    const ofVec2f source(0, 1);
    const ofVec2f sink1(-halfBaseSize, -.5);
    const ofVec2f sink2(halfBaseSize, -.5);
    const ofVec2f center = (source + sink1 + sink2) / 3.0;
    const float bezierOffset = 0.5 * baseSize;
    const float maxInternalOffset = (.25 * source - .5 * center + .25 * sink1).length();
    const float centerStretchFactor = (maxInternalOffset + bezierOffset) / bezierOffset;


    ofVec2f focusPt = isSourceSpline ? ofVec2f(baseSize, 1) : ofVec2f(0, -2);
    float fromFocusAngle = M_PI * (isSourceSpline ? (1.0 + t / 3.0) : ((1.0 + t) / 3.0));
    ofVec2f toPtVector(cosf(fromFocusAngle), sinf(fromFocusAngle));

    float offset = (offsetIndex == 2) ? (.5 * baseSize) : baseSize;
    ofVec2f xy = focusPt + offset * toPtVector;

    if(offsetIndex == 0) {
        //project point on base spline
        ofVec2f projBase = isSourceSegment ? ofVec2f(0,1) : ofVec2f(halfBaseSize, -.5);
        xy = dot(xy, projBase) * projBase;
    }

    //in case we are dealing with the center point:
    if(offsetIndex == -1)
        xy = ofVec2f(0,0);
    
    const ofVec2f cornerTL = source + (sink1 - sink2);
    const ofVec2f cornerTR = source + (sink2 - sink1);
    const ofVec2f cornerB = sink1 + (sink2 - source);

    ofVec2f vecSource = (center - source).normalize();
    ofVec2f vecSink1 = (sink1 - center).normalize();
    ofVec2f vecSink2 = (sink2 - center).normalize();
 
    float traversalDistance = 2. * (center - source).length();
 
    float projSource = dot(xy - source, vecSource);
    float projSink1 = dot(xy - sink1, vecSink1);
    float projSink2 = dot(xy - sink2, vecSink2);
 
    float orSource = cross(xy - source, vecSource);
    float orSink1 = cross(xy - sink1, vecSink1);
    float orSink2 = cross(xy - sink2, vecSink2);
 
    float val1 = projSource / traversalDistance;
    float val2 = 1.0 + projSink1 / traversalDistance;
    float val3 = 1.0 + projSink2 / traversalDistance;

    float offsetX = 0;
    if(ABS(projSource) < .0001)
        offsetX = val1;
    else if(ABS(projSink1) < .0001)
        offsetX = val2;
    else if(ABS(projSink2) < .0001)
        offsetX = val3;
    else {
        float power = 2.0;
        float weight1 = powf(1.0 / ABS(projSource), power);
        float weight2 = powf(1.0 / ABS(projSink1), power);
        float weight3 = powf(1.0 / ABS(projSink2), power);
        float sumWeight = weight1 + weight2 + weight3;
    
        offsetX = (weight1 / sumWeight) * val1
                    + (weight2 / sumWeight) * val2
                    + (weight3 / sumWeight) * val3;
    }
 
 
  ofVec2f to = xy - focusPt;
  float toDist = to.length();
  to /= toDist;
 
  float dist = MAX(0.0, toDist - bezierOffset);
 
  float maxAng = M_PI / 6.;
 
  float angle = acos(dot(to, (center - focusPt).normalize()));
  float maxOffset = baseSize / cos(M_PI / 6.0 - angle) - bezierOffset;
 
  float circDistFrac = dist / (baseSize - bezierOffset);
  float projDistFrac = dist / maxOffset;
 
 
  float angleFrac = 1. - angle / maxAng;
 
 
  float offFactor = pow(projDistFrac, 2.0 + abs(angleFrac) * projDistFrac);
  float offsetY = (1. - offFactor) * circDistFrac + offFactor * projDistFrac;
  offsetY = 1. - offsetY;

  if(isnan(offsetX) || isnan(offsetY))
      cout << "OFFSET VALUE is NaN" << endl;

  return ofVec2f(offsetX - .5, offsetY);
}
Esempio n. 27
0
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
    return MIN(ABS(rcData[axis] - midrc), 500);
}
Esempio n. 28
0
      /* 'Data'   Matrix of data containing observation data in rows, one
       *          row per observation and complying with this format:
       *                    x y z P
       *          Where x,y,z are satellite coordinates in an ECEF system
       *          and P is pseudorange (corrected as much as possible,
       *          specially from satellite clock errors), all expresed
       *          in meters.
       *
       * 'X'      Vector of position solution, in meters. There may be
       *          another solution, that may be accessed with vector
       *          "SecondSolution" if "ChooseOne" is set to "false".
       *
       * Return values:
       *  0  Ok
       * -1  Not enough good data
       * -2  Singular problem
       */
   int Bancroft::Compute( Matrix<double>& Data,
                          Vector<double>& X )
      throw(Exception)
   {

      try
      {

         int N = Data.rows();
         Matrix<double> B(0,4);     // Working matrix

            // Let's test the input data
         if( testInput )
         {

            double satRadius = 0.0;

               // Check each row of B Matrix
            for( int i=0; i < N; i++ )
            {
                  // If Data(i,3) -> Pseudorange is NOT between the allowed
                  // range, then drop line immediately
               if( !( (Data(i,3) >= minPRange) && (Data(i,3) <= maxPRange) ) )
               {
                  continue;
               }

                  // Let's compute distance between Earth center and
                  // satellite position
               satRadius = RSS(Data(i,0), Data(i,1) , Data(i,2));

                  // If satRadius is NOT between the allowed range, then drop
                  // line immediately
               if( !( (satRadius >= minRadius) && (satRadius <= maxRadius) ) )
               {
                  continue;
               }

                  // If everything is ok so far, then extract the good
                  // data row and add it to working matrix
               MatrixRowSlice<double> goodRow(Data,i);
               B = B && goodRow;              

            }

               // Let's redefine "N" and check if we have enough data rows
               // left in a single step
            if( (N = B.rows()) < 4 )
            {
               return -1;  // We need at least 4 data rows
            }

         }  // End of 'if( testInput )...'
         else
         {
               // No input filtering. Working matrix (B) and
               // input matrix (Data) are equal
            B = Data;
         }


         Matrix<double> BT=transpose(B);
         Matrix<double> BTBI(4,4), M(4,4,0.0);
         Vector<double> aux(4), alpha(N), solution1(4), solution2(4);

            // Temporary storage for BT*B. It will be inverted later
         BTBI = BT * B;

            // Let's try to invert BTB matrix
         try
         {
            BTBI = inverseChol( BTBI ); 
         }
         catch(...)
         {
            return -2;
         }

            // Now, let's compute alpha vector
         for( int i=0; i < N; i++ )
         {
               // First, fill auxiliar vector with corresponding satellite
               // position and pseudorange
            aux(0) = B(i,0);
            aux(1) = B(i,1);
            aux(2) = B(i,2);
            aux(3) = B(i,3);
            alpha(i) = 0.5 * Minkowski(aux, aux);
         }

         Vector<double> tau(N,1.0), BTBIBTtau(4), BTBIBTalpha(4);

         BTBIBTtau = BTBI * BT * tau;
         BTBIBTalpha = BTBI * BT * alpha;

            // Now, let's find the coeficients of the second order-equation
         double a(Minkowski(BTBIBTtau, BTBIBTtau));
         double b(2.0 * (Minkowski(BTBIBTtau, BTBIBTalpha) - 1.0));
         double c(Minkowski(BTBIBTalpha, BTBIBTalpha));

            // Calculate discriminant and exit if negative
         double discriminant = b*b - 4.0 * a * c;
         if (discriminant < 0.0)
         {
            return -2;
         }

            // Find possible DELTA values
         double DELTA1 = ( -b + SQRT(discriminant) ) / ( 2.0 * a );
         double DELTA2 = ( -b - SQRT(discriminant) ) / ( 2.0 * a );

            // We need to define M matrix
         M(0,0) = 1.0;
         M(1,1) = 1.0;
         M(2,2) = 1.0;
         M(3,3) = - 1.0;

            // Find possible position solutions with their implicit radii
         solution1 = M *  BTBI * ( BT * DELTA1 * tau + BT * alpha );
         double radius1(RSS(solution1(0), solution1(1), solution1(2)));

         solution2 = M *  BTBI * ( BT * DELTA2 * tau + BT * alpha );
         double radius2(RSS(solution2(0), solution2(1), solution2(2)));

            // Let's choose the right solution
         if ( ChooseOne )
         {
            if ( ABS(CloseTo-radius1) < ABS(CloseTo-radius2) )
            {
               X = solution1;
            }
            else
            {
               X = solution2;
            }
         }
         else
         {
               // Both solutions will be reported
            X = solution1;
            SecondSolution = solution2;
         }
     
         return 0;

      }  // end of first "try"
      catch(Exception& e)
      {
         GPSTK_RETHROW(e);
      }
   }  // end Bancroft::Compute()
Esempio n. 29
0
static void
SDL_BlendLine_ARGB8888(SDL_Surface * dst, int x1, int y1, int x2, int y2,
                       SDL_BlendMode blendMode, Uint8 _r, Uint8 _g, Uint8 _b, Uint8 _a,
                       SDL_bool draw_end)
{
    unsigned r, g, b, a, inva;

    if (blendMode == SDL_BLENDMODE_BLEND || blendMode == SDL_BLENDMODE_ADD) {
        r = DRAW_MUL(_r, _a);
        g = DRAW_MUL(_g, _a);
        b = DRAW_MUL(_b, _a);
        a = _a;
    } else {
        r = _r;
        g = _g;
        b = _b;
        a = _a;
    }
    inva = (a ^ 0xff);

    if (y1 == y2) {
        switch (blendMode) {
        case SDL_BLENDMODE_BLEND:
            HLINE(Uint32, DRAW_SETPIXEL_BLEND_ARGB8888, draw_end);
            break;
        case SDL_BLENDMODE_ADD:
            HLINE(Uint32, DRAW_SETPIXEL_ADD_ARGB8888, draw_end);
            break;
        case SDL_BLENDMODE_MOD:
            HLINE(Uint32, DRAW_SETPIXEL_MOD_ARGB8888, draw_end);
            break;
        default:
            HLINE(Uint32, DRAW_SETPIXEL_ARGB8888, draw_end);
            break;
        }
    } else if (x1 == x2) {
        switch (blendMode) {
        case SDL_BLENDMODE_BLEND:
            VLINE(Uint32, DRAW_SETPIXEL_BLEND_ARGB8888, draw_end);
            break;
        case SDL_BLENDMODE_ADD:
            VLINE(Uint32, DRAW_SETPIXEL_ADD_ARGB8888, draw_end);
            break;
        case SDL_BLENDMODE_MOD:
            VLINE(Uint32, DRAW_SETPIXEL_MOD_ARGB8888, draw_end);
            break;
        default:
            VLINE(Uint32, DRAW_SETPIXEL_ARGB8888, draw_end);
            break;
        }
    } else if (ABS(x1 - x2) == ABS(y1 - y2)) {
        switch (blendMode) {
        case SDL_BLENDMODE_BLEND:
            DLINE(Uint32, DRAW_SETPIXEL_BLEND_ARGB8888, draw_end);
            break;
        case SDL_BLENDMODE_ADD:
            DLINE(Uint32, DRAW_SETPIXEL_ADD_ARGB8888, draw_end);
            break;
        case SDL_BLENDMODE_MOD:
            DLINE(Uint32, DRAW_SETPIXEL_MOD_ARGB8888, draw_end);
            break;
        default:
            DLINE(Uint32, DRAW_SETPIXEL_ARGB8888, draw_end);
            break;
        }
    } else {
        switch (blendMode) {
        case SDL_BLENDMODE_BLEND:
            AALINE(x1, y1, x2, y2,
                   DRAW_SETPIXELXY_BLEND_ARGB8888, DRAW_SETPIXELXY_BLEND_ARGB8888,
                   draw_end);
            break;
        case SDL_BLENDMODE_ADD:
            AALINE(x1, y1, x2, y2,
                   DRAW_SETPIXELXY_ADD_ARGB8888, DRAW_SETPIXELXY_ADD_ARGB8888,
                   draw_end);
            break;
        case SDL_BLENDMODE_MOD:
            AALINE(x1, y1, x2, y2,
                   DRAW_SETPIXELXY_MOD_ARGB8888, DRAW_SETPIXELXY_MOD_ARGB8888,
                   draw_end);
            break;
        default:
            AALINE(x1, y1, x2, y2,
                   DRAW_SETPIXELXY_ARGB8888, DRAW_SETPIXELXY_BLEND_ARGB8888,
                   draw_end);
            break;
        }
    }
}
Esempio n. 30
0
int invertMatrix(Matrix m, int printErrors)
{
  int ok = 1;
  int i,icol=0,irow=0,j,k,l,ll;
  float big,dum,pivinv;
  
  int n=4;

  if (indxc==0) {
    indxr = new int[4];
    indxc = new int[4];
    ipiv  = new int[4];
    a     = new float*[4];
    if (!a || !ipiv) printf("new fails - memory problem\n");
  }
  for (j=0;j<n;j++) a[j] = &(m[j][0]);

  for (j=0;j<n;j++) ipiv[j]=0;
  for (i=0;i<n;i++) {
    big=0.0;
    for (j=0;j<n;j++)
      if (ipiv[j] != 1)
        for (k=0;k<n;k++) {
          if (ipiv[k] == 0) {
            if (fabs(a[j][k]) >= big) {
              big=ABS(a[j][k]);
              irow=j;
              icol=k;
            }
          } else if (ipiv[k] > 1) {
          		if (printErrors) printf("GAUSSJ: Singular Matrix-1\n");
          		ok =0;
          	}
        }
    ++(ipiv[icol]);
    if (irow != icol) {
      for (l=0;l<n;l++) SWAP(a[irow][l],a[icol][l])
    }
    indxr[i]=irow;
    indxc[i]=icol;
    if (a[icol][icol] == 0.0) {
    	ok =0;
    	if (printErrors) printf("GAUSSJ: Singular Matrix-2");
    }
    pivinv=1.0f/a[icol][icol];
    a[icol][icol]=1.0;
    for (l=0;l<n;l++) a[icol][l] *= pivinv;
    for (ll=0;ll<n;ll++)
      if (ll != icol) {
        dum=a[ll][icol];
        a[ll][icol]=0.0;
        for (l=0;l<n;l++) a[ll][l] -= a[icol][l]*dum;
      }
  }
  for (l=n-1;l>=0;l--) {
    if (indxr[l] != indxc[l])
      for (k=0;k<n;k++)
        SWAP(a[k][indxr[l]],a[k][indxc[l]]);
  }
  return ok;
}