/**
@param[in] vetrices array of vetrices (so far only for 3)
@param[in] E material constatn E
@param[in] mi material constatn mi
@param[in] fs volume force vector
@param[out] stifMat stifness matrix (array 36 long)
@param[out]
*/
void elastLoc(Point *vetrices[], PetscReal E, PetscReal mi, PetscReal *fs,
PetscReal *stifMat, PetscReal *bL) {
PetscReal
T[] =
{ 1, 1, 1, vetrices[0]->x, vetrices[1]->x, vetrices[2]->x, vetrices[0]->y, vetrices[1]->y, vetrices[2]->y };
PetscReal phiGrad[] = { vetrices[1]->y - vetrices[2]->y, vetrices[2]->x
- vetrices[1]->x, vetrices[2]->y - vetrices[0]->y, vetrices[0]->x
- vetrices[2]->x, vetrices[0]->y - vetrices[1]->y, vetrices[1]->x
- vetrices[0]->x
};
//PetscPrintf(PETSC_COMM_SELF, "DET:%e \n", matrixDet(T, 3));
PetscReal detT = fabs(matrixDet(T, 3));
PetscReal phiGradT[6];
matrixTranspose(phiGrad, 3, 2, phiGradT);
double Cmu[] = { 2, 0, 0, 0, 2, 0, 0, 0, 1 };
//double Clm[] = { 1, 1, 0, 1, 1, 0, 0, 0, 0 }; Why is this never used?
PetscReal C[] = { 1 - mi, mi, 0, mi, 1 - mi, 0, 0, 0, (1 - 2 * mi) / 2 };
matrixSum(C, E / ((1 + mi) * (1 - 2 * mi)), Cmu, 0, C, 3, 3);
double R[18], RT[18];
for (int i = 0; i < 18; i++)
R[i] = 0;
int idxR1[] = { 0, 2 };
int idxC1[] = { 0, 2, 4 };
int idxR2[] = { 2, 1 };
int idxC2[] = { 1, 3, 5 };
matrixSetSub(R, 3, 6, idxR1, 2, idxC1, 3, phiGradT);
matrixSetSub(R, 3, 6, idxR2, 2, idxC2, 3, phiGradT);
matrixTranspose(R, 3, 6, RT);
PetscReal temp[18];
matrixMult(C, 3, 3, R, 3, 6, temp);
matrixMult(RT, 6, 3, temp, 3, 6, stifMat);
matrixSum(stifMat, 1 / (2 * detT), stifMat, 0, stifMat, 6, 6);
//PetscPrintf(PETSC_COMM_SELF, "%e %e \n", fs[0], fs[1]);
//Volume Force
for (int i = 0; i < 3; i++) {
bL[i * 2] = detT / 6 * fs[0];
bL[i * 2 + 1] = detT / 6 * fs[1];
}
//@TODO Edge Force!!!
}
void ParamContainerEmissions::setGaussianSIGMA(double **sigma) {
int i,j;
for(i=0; i<D; i++) {
for(j=0; j<D; j++) {
this->sigma[i][j] = sigma[i][j];
this->inverseSigma[i][j] = sigma[i][j];
}
}
inverse(inverseSigma, this->D);
this->determinant = matrixDet(sigma, this->D);
}
/**
* 01) ParamContainerEmissions Constructor for MULTIVARIATEGAUSSIAN
*
*/
ParamContainerEmissions::ParamContainerEmissions(double **mu, double **sigma, double regularize, int D, int* start, int updateCov, int sharedCov) {
//printf("create container");
//cout<<"create container";
this->logCovPrior = 0;
this->updateCov = updateCov;
this->sharedCov = sharedCov;
this->whichone = MULTIVARIATEGAUSSIAN;
this->mu = mu;
this->sigma = sigma;
this->regularize = regularize;
this->D = D;
this->start=start;
this->inverseSigma = allocateNumericMatrix(D, D);
int i,j;
for(i=0; i<D; i++) {
for(j=0; j<D; j++) {
this->inverseSigma[i][j] = this->sigma[i][j];
//cout<< this->inverseSigma[i][j]<<" ";
}
//cout<<endl;
}
//cout<<endl;
inverse(this->inverseSigma, D);
//printf("whichone: %d dimension %d , arraystartsize %d \n", this->whichone, D, ARRAYSIZE(start));
//LapackInvAndDet(this->inverseSigma, D);
this->determinant = matrixDet(sigma, D);
//cout<<"determinant "<< this->determinant<<endl;
int mem = sizeof(double*)*D + sizeof(double)*1 + 2*sizeof(double*)*D + 2*sizeof(double)*D;
if(DEBUG_MEMORY) {
//printf("new->ParamContainerEmissions:MULTIVARIATEGAUSSIAN ; (%d bytes) ", mem);
}
//cout<<"new->ParamContainerEmissions:MULTIVARIATEGAUSSIAN ; (%d bytes) "<< mem<<endl;;
}
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();
}
