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();
}
