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; } }
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(); }
bool areSticksInApModePosition(uint16_t ap_mode) { return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode; }
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_ */
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) ; }
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]; }
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; }
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); }
/*------------------------------------------------------------------*/ 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 } } }
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); }
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; }
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; }
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; }
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 }
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; }
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]; }
/*---------------------------------------------------------------------------*/ 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); }
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 */
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; }
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 ); }
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); }
/* 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; } }
void Functions::fabs(Aurora::NWScript::FunctionContext &ctx) { ctx.getReturn() = ABS(ctx.getParams()[0].getFloat()); }
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); }
/* 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); }
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { return MIN(ABS(rcData[axis] - midrc), 500); }
/* '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()
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; } } }
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; }