static int adjust_vertices(privpath_t *pp) { int m = pp->m; int *po = pp->po; int n = pp->len; point_t *pt = pp->pt; int x0 = pp->x0; int y0 = pp->y0; dpoint_t *ctr = NULL; /* ctr[m] */ dpoint_t *dir = NULL; /* dir[m] */ quadform_t *q = NULL; /* q[m] */ double v[3]; double d; int i, j, k, l; dpoint_t s; int r; SAFE_MALLOC(ctr, m, dpoint_t); SAFE_MALLOC(dir, m, dpoint_t); SAFE_MALLOC(q, m, quadform_t); r = privcurve_init(&pp->curve, m); if (r) { goto malloc_error; } /* calculate "optimal" point-slope representation for each line segment */ for (i=0; i<m; i++) { j = po[mod(i+1,m)]; j = mod(j-po[i],n)+po[i]; pointslope(pp, po[i], j, &ctr[i], &dir[i]); } /* represent each line segment as a singular quadratic form; the distance of a point (x,y) from the line segment will be (x,y,1)Q(x,y,1)^t, where Q=q[i]. */ for (i=0; i<m; i++) { d = sq(dir[i].x) + sq(dir[i].y); if (d == 0.0) { for (j=0; j<3; j++) { for (k=0; k<3; k++) { q[i][j][k] = 0; } } } else { v[0] = dir[i].y; v[1] = -dir[i].x; v[2] = - v[1] * ctr[i].y - v[0] * ctr[i].x; for (l=0; l<3; l++) { for (k=0; k<3; k++) { q[i][l][k] = v[l] * v[k] / d; } } } } /* now calculate the "intersections" of consecutive segments. Instead of using the actual intersection, we find the point within a given unit square which minimizes the square distance to the two lines. */ for (i=0; i<m; i++) { quadform_t Q; dpoint_t w; double dx, dy; double det; double min, cand; /* minimum and candidate for minimum of quad. form */ double xmin, ymin; /* coordinates of minimum */ int z; /* let s be the vertex, in coordinates relative to x0/y0 */ s.x = pt[po[i]].x-x0; s.y = pt[po[i]].y-y0; /* intersect segments i-1 and i */ j = mod(i-1,m); /* add quadratic forms */ for (l=0; l<3; l++) { for (k=0; k<3; k++) { Q[l][k] = q[j][l][k] + q[i][l][k]; } } while(1) { /* minimize the quadratic form Q on the unit square */ /* find intersection */ #ifdef HAVE_GCC_LOOP_BUG /* work around gcc bug #12243 */ free(NULL); #endif det = Q[0][0]*Q[1][1] - Q[0][1]*Q[1][0]; if (det != 0.0) { w.x = (-Q[0][2]*Q[1][1] + Q[1][2]*Q[0][1]) / det; w.y = ( Q[0][2]*Q[1][0] - Q[1][2]*Q[0][0]) / det; break; } /* matrix is singular - lines are parallel. Add another, orthogonal axis, through the center of the unit square */ if (Q[0][0]>Q[1][1]) { v[0] = -Q[0][1]; v[1] = Q[0][0]; } else if (Q[1][1]) { v[0] = -Q[1][1]; v[1] = Q[1][0]; } else { v[0] = 1; v[1] = 0; } d = sq(v[0]) + sq(v[1]); v[2] = - v[1] * s.y - v[0] * s.x; for (l=0; l<3; l++) { for (k=0; k<3; k++) { Q[l][k] += v[l] * v[k] / d; } } } dx = fabs(w.x-s.x); dy = fabs(w.y-s.y); if (dx <= .5 && dy <= .5) { pp->curve.vertex[i].x = w.x+x0; pp->curve.vertex[i].y = w.y+y0; continue; } /* the minimum was not in the unit square; now minimize quadratic on boundary of square */ min = quadform(Q, s); xmin = s.x; ymin = s.y; if (Q[0][0] == 0.0) { goto fixx; } for (z=0; z<2; z++) { /* value of the y-coordinate */ w.y = s.y-0.5+z; w.x = - (Q[0][1] * w.y + Q[0][2]) / Q[0][0]; dx = fabs(w.x-s.x); cand = quadform(Q, w); if (dx <= .5 && cand < min) { min = cand; xmin = w.x; ymin = w.y; } } fixx: if (Q[1][1] == 0.0) { goto corners; } for (z=0; z<2; z++) { /* value of the x-coordinate */ w.x = s.x-0.5+z; w.y = - (Q[1][0] * w.x + Q[1][2]) / Q[1][1]; dy = fabs(w.y-s.y); cand = quadform(Q, w); if (dy <= .5 && cand < min) { min = cand; xmin = w.x; ymin = w.y; } } corners: /* check four corners */ for (l=0; l<2; l++) { for (k=0; k<2; k++) { w.x = s.x-0.5+l; w.y = s.y-0.5+k; cand = quadform(Q, w); if (cand < min) { min = cand; xmin = w.x; ymin = w.y; } } } pp->curve.vertex[i].x = xmin + x0; pp->curve.vertex[i].y = ymin + y0; continue; } free(ctr); free(dir); free(q); return 0; malloc_error: free(ctr); free(dir); free(q); return 1; }
void gibbsOneWayAnova(double *y, int *N, int J, int sumN, int *whichJ, double rscale, int iterations, double *chains, double *CMDE, SEXP debug, int progress, SEXP pBar, SEXP rho) { int i=0,j=0,m=0,Jp1sq = (J+1)*(J+1),Jsq=J*J,Jp1=J+1,npars=0; double ySum[J],yBar[J],sumy2[J],densDelta=0; double sig2=1,g=1; double XtX[Jp1sq], ZtZ[Jsq]; double Btemp[Jp1sq],B2temp[Jsq],tempBetaSq=0; double muTemp[J],oneOverSig2temp=0; double beta[J+1],grandSum=0,grandSumSq=0; double shapeSig2 = (sumN+J*1.0)/2, shapeg = (J+1.0)/2; double scaleSig2=0, scaleg=0; double Xty[J+1],Zty[J]; double logDet=0; double rscaleSq=rscale*rscale; double logSumSingle=0,logSumDouble=0; // for Kahan sum double kahanSumSingle=0, kahanSumDouble=0; double kahanCSingle=0,kahanCDouble=0; double kahanTempT=0, kahanTempY=0; int iOne=1, info; double dZero=0; // progress stuff SEXP sampCounter, R_fcall; int *pSampCounter; PROTECT(R_fcall = lang2(pBar, R_NilValue)); PROTECT(sampCounter = NEW_INTEGER(1)); pSampCounter = INTEGER_POINTER(sampCounter); npars=J+5; GetRNGstate(); // Initialize to 0 AZERO(XtX,Jp1sq); AZERO(ZtZ,Jsq); AZERO(beta,Jp1); AZERO(ySum,J); AZERO(sumy2,J); // Create vectors for(i=0;i<sumN;i++) { j = whichJ[i]; ySum[j] += y[i]; sumy2[j] += y[i]*y[i]; grandSum += y[i]; grandSumSq += y[i]*y[i]; } // create design matrices XtX[0]=sumN; for(j=0;j<J;j++) { XtX[j+1]=N[j]; XtX[(J+1)*(j+1)]=N[j]; XtX[(j+1)*(J+1) + (j+1)] = N[j]; ZtZ[j*J + j] = N[j]; yBar[j] = ySum[j]/(1.0*N[j]); } Xty[0] = grandSum; Memcpy(Xty+1,ySum,J); Memcpy(Zty,ySum,J); // start MCMC for(m=0; m<iterations; m++) { R_CheckUserInterrupt(); //Check progress if(progress && !((m+1)%progress)){ pSampCounter[0]=m+1; SETCADR(R_fcall, sampCounter); eval(R_fcall, rho); //Update the progress bar } // sample beta Memcpy(Btemp,XtX,Jp1sq); for(j=0;j<J;j++){ Btemp[(j+1)*(J+1)+(j+1)] += 1/g; } InvMatrixUpper(Btemp, J+1); internal_symmetrize(Btemp,J+1); for(j=0;j<Jp1sq;j++) Btemp[j] *= sig2; oneOverSig2temp = 1/sig2; F77_CALL(dsymv)("U", &Jp1, &oneOverSig2temp, Btemp, &Jp1, Xty, &iOne, &dZero, beta, &iOne); rmvGaussianC(beta, Btemp, J+1); Memcpy(&chains[npars*m],beta,J+1); // calculate density (Single Standardized) Memcpy(B2temp,ZtZ,Jsq); densDelta = -J*0.5*log(2*M_PI); for(j=0;j<J;j++) { B2temp[j*J+j] += 1/g; muTemp[j] = (ySum[j]-N[j]*beta[0])/sqrt(sig2); } InvMatrixUpper(B2temp, J); internal_symmetrize(B2temp,J); logDet = matrixDet(B2temp,J,J,1, &info); densDelta += -0.5*quadform(muTemp, B2temp, J, 1, J); densDelta += -0.5*logDet; if(m==0){ logSumSingle = densDelta; kahanSumSingle = exp(densDelta); }else{ logSumSingle = logSumSingle + LogOnePlusX(exp(densDelta-logSumSingle)); kahanTempY = exp(densDelta) - kahanCSingle; kahanTempT = kahanSumSingle + kahanTempY; kahanCSingle = (kahanTempT - kahanSumSingle) - kahanTempY; kahanSumSingle = kahanTempT; } chains[npars*m + (J+1) + 0] = densDelta; // calculate density (Double Standardized) densDelta += 0.5*J*log(g); if(m==0){ logSumDouble = densDelta; kahanSumDouble = exp(densDelta); }else{ logSumDouble = logSumDouble + LogOnePlusX(exp(densDelta-logSumDouble)); kahanTempY = exp(densDelta) - kahanCDouble; kahanTempT = kahanSumDouble + kahanTempY; kahanCDouble = (kahanTempT - kahanSumDouble) - kahanTempY; kahanSumDouble = kahanTempT; } chains[npars*m + (J+1) + 1] = densDelta; // sample sig2 tempBetaSq = 0; scaleSig2 = grandSumSq - 2*beta[0]*grandSum + beta[0]*beta[0]*sumN; for(j=0;j<J;j++) { scaleSig2 += -2.0*(yBar[j]-beta[0])*N[j]*beta[j+1] + (N[j]+1/g)*beta[j+1]*beta[j+1]; tempBetaSq += beta[j+1]*beta[j+1]; } scaleSig2 *= 0.5; sig2 = 1/rgamma(shapeSig2,1/scaleSig2); chains[npars*m + (J+1) + 2] = sig2; // sample g scaleg = 0.5*(tempBetaSq/sig2 + rscaleSq); g = 1/rgamma(shapeg,1/scaleg); chains[npars*m + (J+1) + 3] = g; } CMDE[0] = logSumSingle - log(iterations); CMDE[1] = logSumDouble - log(iterations); CMDE[2] = log(kahanSumSingle) - log(iterations); CMDE[3] = log(kahanSumDouble) - log(iterations); UNPROTECT(2); PutRNGstate(); }