// mcmc_helper(im,im_prev,buf,int(xwho),int(xwho2));
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
// trw_bprop_helper2(model, psi_ij_rho, psi_i, rho, maxiter, n, m1, m2, b_i,...
// dm1, dm2, dn, dpsi_i, dpsi_ij);
int i=0;
int nnodes = mapdouble(mxGetField(prhs[i],0,"nnodes"))(0);
int ncliques = mapdouble(mxGetField(prhs[i],0,"ncliques"))(0);
int nvals = mapdouble(mxGetField(prhs[i],0,"nvals"))(0);
MatrixXi pairs = mapdouble(mxGetField(prhs[i],0,"pairs")).cast<int>();
pairs.array() -= 1;
MatrixXi N1 = mapdouble(mxGetField(prhs[i],0,"N1")).cast<int>();
N1.array() -= 1;
MatrixXi N2 = mapdouble(mxGetField(prhs[i],0,"N2")).cast<int>();
N2.array() -= 1;
i++;
MatrixMd psi_ij = mapdouble(prhs[i++]);
MatrixMd psi_i = mapdouble(prhs[i++]);
double rho = mapdouble(prhs[i++])(0);
int maxiter = mapdouble(prhs[i++])(0);
MatrixMd n = mapdouble(prhs[i++]);
MatrixMd m1 = mapdouble(prhs[i++]);
MatrixMd m2 = mapdouble(prhs[i++]);
MatrixMd b_i = mapdouble(prhs[i++]);
MatrixMd mstor = mapdouble(prhs[i++]);
MatrixMd dm1 = mapdouble(prhs[i++]);
MatrixMd dm2 = mapdouble(prhs[i++]);
MatrixMd dn = mapdouble(prhs[i++]);
MatrixMd dpsi_i = mapdouble(prhs[i++]);
MatrixMd dpsi_ij = mapdouble(prhs[i++]);
MatrixMd b_ij0 = mapdouble(prhs[i++]);
MatrixMd db_ij0 = mapdouble(prhs[i++]);
int dorec = mapdouble(prhs[i++]).cast<int>()(0);
int w = mstor.rows()-1;
MatrixXd S (nvals,nvals);
MatrixXd m0(nvals,1);
for(int c=0; c<ncliques; c++){
int i = pairs(c, 0);
int j = pairs(c, 1);
for(int yi=0; yi<nvals; yi++){
for(int yj=0; yj<nvals; yj++){
int index = yi + yj*nvals;
//b_ij0(index,c) = b_ij0(index,c)*psi_i(yi,i)*psi_i(yj,j)*n(yi,i)*n(yj,j)/m1(yi,c)/m2(yj,c);
dpsi_i(yi, i) = dpsi_i(yi, i) + db_ij0(index, c)*b_ij0(index, c)/psi_i(yi, i);
dpsi_i(yj, j) = dpsi_i(yj, j) + db_ij0(index, c)*b_ij0(index, c)/psi_i(yj, j);
dn(yi, i) = dn(yi, i) + db_ij0(index, c)*b_ij0(index, c)/n(yi, i);
dn(yj, j) = dn(yj, j) + db_ij0(index, c)*b_ij0(index, c)/n(yj, j);
dm1(yi, c) = dm1(yi, c) - db_ij0(index, c)*b_ij0(index, c)/m1(yi, c);
dm2(yj, c) = dm2(yj, c) - db_ij0(index, c)*b_ij0(index, c)/m2(yj, c);
}
}
}
for(int i=0; i<b_i.cols(); i++){
for(int yi=0; yi<nvals; yi++){
for(int k=0; k<N1.cols(); k++){
int d = N1(i, k);
if(d==-2) continue;
//n(yi, i) *= m1(yi, d);
dm1(yi,d) += rho * dn(yi,i)*n(yi,i)/m1(yi,d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(i, k);
if(d==-2) continue;
//n(yi, i) *= m2(yi, d);
dm2(yi,d) += rho * dn(yi,i)*n(yi,i)/m2(yi,d);
}
}
}
int reps;
double conv;
for(reps=0; reps<maxiter; reps++){
for(int c0=2*ncliques-1; c0>=0; c0--){
int c, mode;
if(c0<ncliques){
c = c0;
mode = 1;
} else{
c = ncliques - 1 - (c0-ncliques);
mode = 2;
}
int i = pairs(c,0);
int j = pairs(c,1);
if( mode==1 ){
for(int yi=0; yi<nvals; yi++)
n(yi, i) = 1;
for(int k=0; k<N1.cols(); k++){
int d = N1(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m1(yi, d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m2(yi, d);
}
// n.col(i) = n.col(i).array().pow(rho);
if(rho==1){
//nothing
} else if(rho==.5){
n.col(i) = n.col(i).array().sqrt();
} else
n.col(i) = n.col(i).array().pow(rho);
// compute m(y_j)
for(int yj=0; yj<nvals; yj++){
m0(yj) = 0;
for(int yi=0; yi<nvals; yi++){
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yi, i)*n(yi, i)/m1(yi, c);
m0(yj) += S(yi,yj);
}
}
double k = S.sum();
MatrixXd dm0 = dm2.col(c)/k;
dm0.array() -= (dm2.col(c).array()*m0.array()).sum()/(k*k);
MatrixXd dn = 0*n.col(i);
//#pragma omp parallel for num_threads(2)
for(int yj=0; yj<nvals; yj++){
for(int yi=0; yi<nvals; yi++){
int index = yi + yj*nvals;
dpsi_ij(index,c) += dm0(yj)*S(yi,yj)/psi_ij(index,c);
dpsi_i(yi,i) += dm0(yj)*S(yi,yj)/psi_i(yi,i);
dn(yi) += dm0(yj)*S(yi,yj)/n(yi,i);
dm1(yi,c) -= dm0(yj)*S(yi,yj)/m1(yi,c);
}
}
for(int yi=0; yi<nvals; yi++){
for(int k=0; k<N1.cols(); k++){
int d = N1(i, k);
if(d==-2) continue;
dm1(yi,d) += rho*dn(yi)*n(yi,i)/m1(yi,d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(i, k);
if(d==-2) continue;
dm2(yi,d) += rho*dn(yi)*n(yi,i)/m2(yi,d);
}
}
dm2.col(c) *= 0.0;
if( dorec )
for(int yj=nvals-1; yj>=0; yj--){ m2(yj,c)=mstor(w); w--;}
} else{
for( int yj=0; yj<nvals; yj++)
n(yj, j) = 1;
for(int k=0; k<N1.cols(); k++){
int d = N1(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m1(yj, d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m2(yj, d);
}
// n.col(j) = n.col(j).array().pow(rho);
if(rho==1){
//nothing
} else if(rho==.5){
n.col(j) = n.col(j).array().sqrt();
} else
n.col(j) = n.col(j).array().pow(rho);
for(int yi=0; yi<nvals; yi++){
m0(yi) = 0;
for(int yj=0; yj<nvals; yj++){
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yj, j)*n(yj, j)/m2(yj, c);
m0(yi) += S(yi,yj);
}
}
double k = S.sum();
MatrixXd dm0 = dm1.col(c)/k;
dm0.array() -= (dm1.col(c).array()*m0.array()).sum()/(k*k);
MatrixXd dn = 0*n.col(i);
for(int yj=0; yj<nvals; yj++){
for(int yi=0; yi<nvals; yi++){
int index = yi + yj*nvals;
dpsi_ij(index,c) += dm0(yi)*S(yi,yj)/psi_ij(index,c);
dpsi_i(yj,j) += dm0(yi)*S(yi,yj)/psi_i(yj,j);
dn(yj) += dm0(yi)*S(yi,yj)/n(yj,j);
dm2(yj,c) -= dm0(yi)*S(yi,yj)/m2(yj,c);
}
}
for(int yj=0; yj<nvals; yj++){
for(int k=0; k<N1.cols(); k++){
int d = N1(j, k);
if(d==-2) continue;
dm1(yj, d) += rho*dn(yj)*n(yj, j)/m1(yj, d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(j, k);
if(d==-2) continue;
dm2(yj, d) += rho*dn(yj)*n(yj, j)/m2(yj, d);
}
}
dm1.col(c) *= 0.0;
if( dorec )
for(int yi=nvals-1; yi>=0; yi--){ m1(yi,c)=mstor(w); w--;}
}
}
}
}
// mcmc_helper(im,im_prev,buf,int(xwho),int(xwho2));
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
// trw_simple_helper(model,c_ij, psi_i, rho, n, m1, m2);
// need from model struct:
//model =
//
// nnodes: 10000
//ncliques: 20000
// nvals: 2
// pairs: [20000x2 double]
// N1: {10000x1 cell}
// N2: {10000x1 cell}
int i=0;
int nnodes = mapdouble(mxGetField(prhs[i],0,"nnodes"))(0);
int ncliques = mapdouble(mxGetField(prhs[i],0,"ncliques"))(0);
int nvals = mapdouble(mxGetField(prhs[i],0,"nvals"))(0);
MatrixXi pairs = mapdouble(mxGetField(prhs[i],0,"pairs")).cast<int>();
pairs.array() -= 1;
MatrixXi N1 = mapdouble(mxGetField(prhs[i],0,"N1")).cast<int>();
N1.array() -= 1;
MatrixXi N2 = mapdouble(mxGetField(prhs[i],0,"N2")).cast<int>();
N2.array() -= 1;
//vector<MatrixXi> N1;
//vector<MatrixXi> N2;
//mxArray *mxN1 = mxGetField(prhs[i],0,"N1");
//mxArray *mxN2 = mxGetField(prhs[i],0,"N2");
//for(int node=0; node<nnodes; node++){
// N1.push_back( mapdouble(mxGetCell(mxN1,node)).cast<int>() );
// N2.push_back( mapdouble(mxGetCell(mxN2,node)).cast<int>() );
// N1[node].cwise() -= 1;
// N2[node].cwise() -= 1;
//}
i++;
MatrixMd psi_ij = mapdouble(prhs[i++]);
MatrixMd psi_i = mapdouble(prhs[i++]);
double rho = mapdouble(prhs[i++])(0);
int maxiter = mapdouble(prhs[i++])(0);
double damp = mapdouble(prhs[i++])(0);
double convthresh = mapdouble(prhs[i++])(0);
MatrixMd n = mapdouble(prhs[i++]);
MatrixMd m1 = mapdouble(prhs[i++]);
MatrixMd m2 = mapdouble(prhs[i++]);
MatrixMd b_i = mapdouble(prhs[i++]);
MatrixMd b_ij = mapdouble(prhs[i++]);
MatrixMd Z_ij = mapdouble(prhs[i++]);
MatrixXd b_i_save = b_i;
MatrixXd S (nvals,nvals);
MatrixXd m0(nvals,1);
int reps;
double conv;
for(reps=0; reps<maxiter; reps++){
for(int c0=0; c0<ncliques*2; c0++){
int c, mode;
if(c0<ncliques){
c = c0;
mode = 1;
} else{
c = ncliques - 1 - (c0-ncliques);
mode = 2;
}
int i = pairs(c,0);
int j = pairs(c,1);
if( mode==1 ){
for(int yi=0; yi<nvals; yi++)
n(yi, i) = 1;
for(int k=0; k<N1.cols(); k++){
int d = N1(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m1(yi, d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m2(yi, d);
}
for(int yi=0; yi<nvals; yi++)
n(yi, i) = pow(n(yi, i), rho);
// compute m(y_j)
for(int yj=0; yj<nvals; yj++){
m0(yj) = 0;
for(int yi=0; yi<nvals; yi++){
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yi, i)*n(yi, i)/m1(yi, c);
m0(yj) += S(yi,yj);
}
}
double k = S.sum();
m2.col(c) = m0/k;
} else{
for( int yj=0; yj<nvals; yj++)
n(yj, j) = 1;
for(int k=0; k<N1.cols(); k++){
int d = N1(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m1(yj, d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m2(yj, d);
}
for( int yj=0; yj<nvals; yj++)
n(yj, j) = pow(n(yj, j), rho);
for(int yi=0; yi<nvals; yi++){
m0(yi) = 0;
for(int yj=0; yj<nvals; yj++){
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yj, j)*n(yj, j)/m2(yj, c);
m0(yi) += S(yi,yj);
}
}
double k = S.sum();
m1.col(c) = m0/k;
}
}
b_i = n.array()*psi_i.array();
norm_cols(b_i);
conv = (b_i-b_i_save).array().abs().maxCoeff();
//cout << "reps: " << reps << " conv: " << conv << endl;
if(conv < convthresh)
break;
b_i_save = b_i;
}
//cout << "reps: " << reps << " conv: " << conv << endl;
b_ij = psi_ij;
for(int c=0; c<ncliques; c++){
int i = pairs(c,0);
int j = pairs(c,1);
for(int yi=0; yi<nvals; yi++){
for(int yj=0; yj<nvals; yj++){
int index = yi + yj*nvals;
b_ij(index,c) = b_ij(index,c)*psi_i(yi,i)*psi_i(yj,j)*n(yi,i)*n(yj,j)/m1(yi,c)/m2(yj,c);
}
}
}
Z_ij = b_ij.colwise().sum();
for(int c=0; c<ncliques; c++)
b_ij.col(c) = b_ij.col(c)/Z_ij(c);
plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
double *outArray;
outArray = mxGetPr(plhs[0]);
outArray[0] = reps;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
// trw_bprop_helper2(model, psi_ij_rho, psi_i, rho, maxiter, n, m1, m2, b_i,...
// dm1, dm2, dn, dpsi_i, dpsi_ij);
int i=0;
int nnodes = mapdouble(mxGetField(prhs[i],0,"nnodes"))(0);
int ncliques = mapdouble(mxGetField(prhs[i],0,"ncliques"))(0);
int nvals = mapdouble(mxGetField(prhs[i],0,"nvals"))(0);
MatrixXi pairs = mapdouble(mxGetField(prhs[i],0,"pairs")).cast<int>();
pairs.array() -= 1;
MatrixXi N1 = mapdouble(mxGetField(prhs[i],0,"N1")).cast<int>();
N1.array() -= 1;
MatrixXi N2 = mapdouble(mxGetField(prhs[i],0,"N2")).cast<int>();
N2.array() -= 1;
MatrixXi tree2clique = mapdouble(mxGetField(prhs[i],0,"tree2clique")).cast<int>();
tree2clique.array() -= 1;
MatrixXi treeschedule = mapdouble(mxGetField(prhs[i],0,"treeschedule")).cast<int>();
treeschedule.array() -= 1;
i++;
MatrixMd psi_ij = mapdouble(prhs[i++]);
MatrixMd psi_i = mapdouble(prhs[i++]);
double rho = mapdouble(prhs[i++])(0);
int maxiter = mapdouble(prhs[i++])(0);
MatrixMd n = mapdouble(prhs[i++]);
MatrixMd m1 = mapdouble(prhs[i++]);
MatrixMd m2 = mapdouble(prhs[i++]);
MatrixMd b_i = mapdouble(prhs[i++]);
MatrixMd mstor = mapdouble(prhs[i++]);
MatrixMd dm1 = mapdouble(prhs[i++]);
MatrixMd dm2 = mapdouble(prhs[i++]);
MatrixMd dn = mapdouble(prhs[i++]);
MatrixMd dpsi_i = mapdouble(prhs[i++]);
MatrixMd dpsi_ij = mapdouble(prhs[i++]);
MatrixMd b_ij0 = mapdouble(prhs[i++]);
MatrixMd db_ij0 = mapdouble(prhs[i++]);
int dorec = mapdouble(prhs[i++]).cast<int>()(0);
MatrixMi w = mapint32(prhs[i++]);
#pragma omp parallel for num_threads(NTHREAD)
for(int c=0; c<ncliques; c++) {
int i = pairs(c, 0);
int j = pairs(c, 1);
for(int yi=0; yi<nvals; yi++) {
for(int yj=0; yj<nvals; yj++) {
int index = yi + yj*nvals;
//b_ij0(index,c) = b_ij0(index,c)*psi_i(yi,i)*psi_i(yj,j)*n(yi,i)*n(yj,j)/m1(yi,c)/m2(yj,c);
dpsi_i(yi, i) = dpsi_i(yi, i) + db_ij0(index, c)*b_ij0(index, c)/psi_i(yi, i);
dpsi_i(yj, j) = dpsi_i(yj, j) + db_ij0(index, c)*b_ij0(index, c)/psi_i(yj, j);
dn(yi, i) = dn(yi, i) + db_ij0(index, c)*b_ij0(index, c)/n(yi, i);
dn(yj, j) = dn(yj, j) + db_ij0(index, c)*b_ij0(index, c)/n(yj, j);
dm1(yi, c) = dm1(yi, c) - db_ij0(index, c)*b_ij0(index, c)/m1(yi, c);
dm2(yj, c) = dm2(yj, c) - db_ij0(index, c)*b_ij0(index, c)/m2(yj, c);
}
}
}
#pragma omp parallel for num_threads(NTHREAD)
for(int i=0; i<b_i.cols(); i++) {
for(int yi=0; yi<nvals; yi++) {
for(int k=0; k<N1.cols(); k++) {
int d = N1(i, k);
if(d==-2) continue;
//n(yi, i) *= m1(yi, d);
dm1(yi,d) += rho * dn(yi,i)*n(yi,i)/m1(yi,d);
}
for(int k=0; k<N2.cols(); k++) {
int d = N2(i, k);
if(d==-2) continue;
//n(yi, i) *= m2(yi, d);
dm2(yi,d) += rho * dn(yi,i)*n(yi,i)/m2(yi,d);
}
}
}
//tree_ncliques = sum(double(model.tree2clique>0),1);
//ntree = length(tree_ncliques);
int ntree = tree2clique.cols();
MatrixXi tree_ncliques = MatrixXi::Zero(ntree,1);
for(int tree=0; tree<tree2clique.cols(); tree++)
for(i=0; i<tree2clique.rows(); i++)
if(tree2clique(i,tree) != -1)
tree_ncliques(tree)++;
int reps;
double conv;
for(reps=0; reps<maxiter; reps++) {
// must re-order blocks
//for(int block=0; block<treeschedule.cols(); block++){
for(int block=treeschedule.cols()-1; block>=0; block--) {
// need not re-order trees, but why not...
// helps if someone specifies not parallel trees
// for(int treenum=treeschedule.rows()-1; treenum>=0; treenum--){
#pragma omp parallel for schedule(dynamic) num_threads(NTHREAD)
for(int treenum=0; treenum<treeschedule.rows(); treenum++) {
MatrixXd S (nvals,nvals);
MatrixXd m0(nvals,1);
int tree = treeschedule(treenum,block);
if(tree==-1)
continue;
int ncliques = tree_ncliques(tree);
for(int c0=2*ncliques-1; c0>=0; c0--) {
int c, mode;
if(c0<ncliques) {
c = tree2clique(c0,tree);
mode = 1;
} else {
c = tree2clique(ncliques - 1 - (c0-ncliques),tree);
mode = 2;
}
//cout << "BW c " << c << " mode " << mode << endl;
int i = pairs(c,0);
int j = pairs(c,1);
if( mode==1 ) {
for(int yi=0; yi<nvals; yi++)
n(yi, i) = 1;
for(int k=0; k<N1.cols(); k++) {
int d = N1(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m1(yi, d);
}
for(int k=0; k<N2.cols(); k++) {
int d = N2(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m2(yi, d);
}
// n.col(i) = n.col(i).array().pow(rho);
if(rho==1) {
//nothing
} else if(rho==.5) {
n.col(i) = n.col(i).array().sqrt();
} else
n.col(i) = n.col(i).array().pow(rho);
// compute m(y_j)
for(int yj=0; yj<nvals; yj++) {
m0(yj) = 0;
for(int yi=0; yi<nvals; yi++) {
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yi, i)*n(yi, i)/m1(yi, c);
m0(yj) += S(yi,yj);
}
}
double k = S.sum();
MatrixXd dm0 = dm2.col(c)/k;
dm0.array() -= (dm2.col(c).array()*m0.array()).sum()/(k*k);
MatrixXd dn = 0*n.col(i);
for(int yj=0; yj<nvals; yj++) {
for(int yi=0; yi<nvals; yi++) {
int index = yi + yj*nvals;
dpsi_ij(index,c) += dm0(yj)*S(yi,yj)/psi_ij(index,c);
dpsi_i(yi,i) += dm0(yj)*S(yi,yj)/psi_i(yi,i);
dn(yi) += dm0(yj)*S(yi,yj)/n(yi,i);
dm1(yi,c) -= dm0(yj)*S(yi,yj)/m1(yi,c);
}
}
for(int yi=0; yi<nvals; yi++) {
for(int k=0; k<N1.cols(); k++) {
int d = N1(i, k);
if(d==-2) continue;
dm1(yi,d) += rho*dn(yi)*n(yi,i)/m1(yi,d);
}
for(int k=0; k<N2.cols(); k++) {
int d = N2(i, k);
if(d==-2) continue;
dm2(yi,d) += rho*dn(yi)*n(yi,i)/m2(yi,d);
}
}
dm2.col(c) *= 0.0;
if( dorec )
for(int yj=nvals-1; yj>=0; yj--) {
w(tree)=w(tree)-1;
m2(yj,c)=mstor(w(tree),tree);
}
} else if( mode==2 ) {
for( int yj=0; yj<nvals; yj++)
n(yj, j) = 1;
for(int k=0; k<N1.cols(); k++) {
int d = N1(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m1(yj, d);
}
for(int k=0; k<N2.cols(); k++) {
int d = N2(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m2(yj, d);
}
// n.col(j) = n.col(j).array().pow(rho);
if(rho==1) {
//nothing
} else if(rho==.5) {
n.col(j) = n.col(j).array().sqrt();
} else
n.col(j) = n.col(j).array().pow(rho);
for(int yi=0; yi<nvals; yi++) {
m0(yi) = 0;
for(int yj=0; yj<nvals; yj++) {
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yj, j)*n(yj, j)/m2(yj, c);
m0(yi) += S(yi,yj);
}
}
double k = S.sum();
MatrixXd dm0 = dm1.col(c)/k;
dm0.array() -= (dm1.col(c).array()*m0.array()).sum()/(k*k);
MatrixXd dn = 0*n.col(i);
for(int yj=0; yj<nvals; yj++) {
for(int yi=0; yi<nvals; yi++) {
int index = yi + yj*nvals;
dpsi_ij(index,c) += dm0(yi)*S(yi,yj)/psi_ij(index,c);
dpsi_i(yj,j) += dm0(yi)*S(yi,yj)/psi_i(yj,j);
dn(yj) += dm0(yi)*S(yi,yj)/n(yj,j);
dm2(yj,c) -= dm0(yi)*S(yi,yj)/m2(yj,c);
}
}
for(int yj=0; yj<nvals; yj++) {
for(int k=0; k<N1.cols(); k++) {
int d = N1(j, k);
if(d==-2) continue;
dm1(yj, d) += rho*dn(yj)*n(yj, j)/m1(yj, d);
}
for(int k=0; k<N2.cols(); k++) {
int d = N2(j, k);
if(d==-2) continue;
dm2(yj, d) += rho*dn(yj)*n(yj, j)/m2(yj, d);
}
}
dm1.col(c) *= 0.0;
if( dorec )
for(int yi=nvals-1; yi>=0; yi--) {
w(tree)=w(tree)-1;
m1(yi,c)=mstor(w(tree),tree);
}
}
}
}
}
}
}
real
calc_phi_SOR
(
real * const phi,
const real * const u,
const real * const v,
const real dt,
const real beta,
const Mesh & mesh
){
real sum_for_numerator = 0.0;
real sum_for_denominator = 0.0;
boundary_update_phi( phi, mesh );
const int k = mesh.hn() ;
for ( int j = mesh.hn() ; j < mesh.hnf(1) ; j++ ) {
for ( int i = mesh.hn() ; i < mesh.hnf(0) ; i++ ) {
const int id = mesh.id(i,j,k);
//if ( i != mesh.hn() || j != mesh.hn() ) {
const int id_e = mesh.id(i+1,j,k);
const int id_w = mesh.id(i-1,j,k);
const int id_n = mesh.id(i,j+1,k);
const int id_s = mesh.id(i,j-1,k);
const real B_x = 1.0 / std::pow(mesh.delta(0), 2);
const real B_y = 1.0 / std::pow(mesh.delta(1), 2);
const real B_ij = 2.0 * ( B_x + B_y );
const real psi = psi_ij( u, v, id, dt, mesh );
const real nabla_phi = B_y * ( phi[id_s] + phi[id_n] )
+ B_x * ( phi[id_w] + phi[id_e] )
- B_ij * phi[id];
const real adjust = nabla_phi - psi;
phi[id] += beta / B_ij * adjust;
sum_for_numerator += std::pow(nabla_phi - psi, 2);
sum_for_denominator += std::pow(psi, 2);
//} else {
//phi[id] = 0.0;
//}
}}
boundary_update_phi( phi, mesh );
const int nn = mesh.n(0) * mesh.n(1);
const real numerator = calc_norm( sum_for_numerator, nn );
const real denominator = calc_norm( sum_for_denominator, nn );
return numerator / denominator;
}
