void Precon_Stokes(Element_List *U, Bsystem *B, double *r, double *z)
{
switch(B->Precon)
{
case Pre_Diag:
dvmul(B->Pmat->info.diag.ndiag,B->Pmat->info.diag.idiag,1,r,1,z,1);
break;
case Pre_Block:
{
register int i;
int nedge = B->Pmat->info.block.nedge;
int info,l,cnt;
double *tmp = dvector(0,LGmax-1);
static double *pinv;
dcopy(B->nsolve,r,1,z,1);
#ifdef BVERT
dpptrs('U',B->Pmat->info.block.nvert,1,
B->Pmat->info.block.ivert,z,
B->Pmat->info.block.nvert,info);
#else
dvmul(B->Pmat->info.block.nvert,B->Pmat->info.block.ivert,1,
z,1,z,1);
#endif
cnt = B->Pmat->info.block.nvert;
for(i = 0; i < nedge; ++i)
{
l = B->Pmat->info.block.Ledge[i];
dpptrs('U',l,1,B->Pmat->info.block.iedge[i],z+cnt,l,info);
cnt += l;
}
if(!pinv)
{
pinv = dvector(0,B->nsolve-cnt-1);
for(i = 0; i < B->nsolve-cnt; ++i)
pinv[i] = U->flist[i]->get_1diag_massmat(0);
dvrecp(B->nsolve-cnt,pinv,1,pinv,1);
}
/* finally multiply pressure dof by inverse of mass matrix */
dvmul(B->nsolve-cnt,pinv,1,r+cnt,1,z+cnt,1);
free(tmp);
}
break;
case Pre_None:
dcopy(B->nsolve,r,1,z,1);
break;
case Pre_Diag_Stokes:
{
register int i,j,k;
double *tmp = dvector(0,B->nglobal-1),*sc,*b,*p,*wk;
int eDIM = U->fhead->dim();
int **bmap = B->bmap;
int nel = B->nel,asize,Nbmodes,info;
if(eDIM == 2)
wk = dvector(0,eDIM*4*LGmax);
else
wk = dvector(0,eDIM*6*LGmax*LGmax);
dzero(B->nglobal,tmp,1);
dcopy(B->nsolve-nel,r,1,tmp,1);
/* multiply by invc */
dvmul(B->nsolve-nel,B->Pmat->info.diagst.idiag,1,tmp,1,tmp,1);
/* multiply by b^T */
sc = B->signchange;
p = z + B->nsolve-nel;
dcopy(nel,r+B->nsolve-nel,1,p,1);
dneg (nel-1,p+1,1);
/* start at 1 to omit singular pressure point */
if(!(B->singular))
error_msg(issue in setting singular modes in Stokes_Precon
not resolved);
for(j = 1; j < nel; ++j)
{
Nbmodes = U->flist[j]->Nbmodes;
asize = Nbmodes*eDIM+1;
dzero (asize,wk,1);
dgathr(eDIM*Nbmodes, tmp, bmap[j], wk);
dvmul (eDIM*Nbmodes, sc, 1, wk, 1, wk, 1);
b = B->Gmat->a[U->flist[j]->geom->id] + asize*(asize-1)/2;
p[j] += ddot(asize-1,b,1,wk,1);
sc += asize;
}
/* solve for p */
dpptrs('L', nel, 1, B->Pmat->info.diagst.binvcb,p,nel,info);
/* generate initial vector as tmp = B p */
sc = B->signchange;
dzero(B->nglobal,tmp,1);
/* start from 1 due to singular pressure system */
for(k = 1; k < nel; ++k)
{
Nbmodes = U->flist[k]->Nbmodes;
asize = Nbmodes*eDIM+1;
b = B->Gmat->a[U->flist[k]->geom->id] + asize*(asize-1)/2;
dsmul(asize-1,p[k],b,1,wk,1);
for(i = 0; i < eDIM*Nbmodes; ++i)
tmp[bmap[k][i]] += sc[i]*wk[i];
sc += asize;
}
/* set z = r - tmp */
dvsub(B->nsolve-nel,r,1,tmp,1,z,1);
/* u = invc*z */
dvmul(B->nsolve-nel,B->Pmat->info.diagst.idiag,1,z,1,z,1);
free(tmp);
free(wk);
}
break;
case Pre_Block_Stokes:
{
register int i,j,k;
double *tmp = dvector(0,B->nglobal-1),*sc,*b,*p,*wk;
int eDIM = U->fhead->dim(),cnt,l;
int **bmap = B->bmap;
int nel = B->nel,asize,Nbmodes,info;
int nedge = B->Pmat->info.blockst.nedge;
if(eDIM == 2)
wk = dvector(0,eDIM*4*LGmax);
else
wk = dvector(0,eDIM*6*LGmax*LGmax);
dzero(B->nglobal,tmp,1);
dcopy(B->nsolve-nel,r,1,tmp,1);
/* multiply by invc */
#ifdef BVERT
dpptrs('U',B->Pmat->info.blockst.nvert,1,
B->Pmat->info.blockst.ivert,tmp,
B->Pmat->info.blockst.nvert,info);
#else
dvmul(B->Pmat->info.blockst.nvert,B->Pmat->info.blockst.ivert,1,
tmp,1,tmp,1);
#endif
cnt = B->Pmat->info.blockst.nvert;
for(i = 0; i < nedge; ++i)
{
l = B->Pmat->info.blockst.Ledge[i];
dpptrs('U',l,1,B->Pmat->info.blockst.iedge[i],tmp+cnt,l,
info);
cnt += l;
}
/* multiply by b^T */
sc = B->signchange;
p = z + B->nsolve-nel;
dcopy(nel,r+B->nsolve-nel,1,p,1);
dneg(nel-1,p+1,1);
if(!(B->singular))
error_msg(issue in setting singular modes in Stokes_Precon
not resolved);
/* start at 1 to omit singular pressure point */
for(j = 1; j < nel; ++j)
{
Nbmodes = U->flist[j]->Nbmodes;
asize = Nbmodes*eDIM+1;
dzero (asize,wk,1);
dgathr(eDIM*Nbmodes, tmp, bmap[j], wk);
dvmul (eDIM*Nbmodes, sc, 1, wk, 1, wk, 1);
b = B->Gmat->a[U->flist[j]->geom->id] + asize*(asize-1)/2;
p[j] += ddot(asize-1,b,1,wk,1);
sc += asize;
}
/* solve for p */
dpptrs('L', nel, 1, B->Pmat->info.blockst.binvcb,p,nel,info);
/* generate initial vector as tmp = B p */
sc = B->signchange;
dzero(B->nglobal,tmp,1);
/* start from 1 due to singular pressure system */
for(k = 1; k < nel; ++k)
{
Nbmodes = U->flist[k]->Nbmodes;
asize = Nbmodes*eDIM+1;
b = B->Gmat->a[U->flist[k]->geom->id] + asize*(asize-1)/2;
dsmul(asize-1,p[k],b,1,wk,1);
for(i = 0; i < eDIM*Nbmodes; ++i)
tmp[bmap[k][i]] += sc[i]*wk[i];
sc += asize;
}
/* set z = r - tmp */
dvsub(B->nsolve-nel,r,1,tmp,1,z,1);
/* u = invc*z */
#ifdef BVERT
dpptrs('U',B->Pmat->info.blockst.nvert,1,
B->Pmat->info.blockst.ivert,z,
B->Pmat->info.blockst.nvert,info);
#else
dvmul(B->Pmat->info.blockst.nvert,B->Pmat->info.blockst.ivert,1,
z,1,z,1);
#endif
cnt = B->Pmat->info.blockst.nvert;
for(i = 0; i < nedge; ++i)
{
l = B->Pmat->info.blockst.Ledge[i];
dpptrs('U',l,1,B->Pmat->info.blockst.iedge[i],z+cnt,l,info);
cnt += l;
}
free(tmp);
free(wk);
}
break;
case Pre_SSOR:
break;
default:
fprintf(stderr,"Preconditioner (Precon=%d) not known\n",B->Precon);
exit(-1);
}
}
static void Bsolve_Stokes_PCG(Element_List **V, Bsystem **B, double *p)
{
int eDIM = V[0]->flist[0]->dim();
const int nsolve = B[eDIM]->nsolve;
int iter = 0;
double tolcg, alpha, beta, eps, rtz, rtz_old, epsfac;
static double *u0 = (double*)0;
static double *u = (double*)0;
static double *r = (double*)0;
static double *w = (double*)0;
static double *z = (double*)0, **wk;
static int nsol = 0, nglob = 0;
if(nsolve > nsol)
{
if(nsol)
{
free(u0);
free(u);
free(r);
free(z);
free_dmatrix(wk,0,0);
}
/* Temporary arrays */
u0 = dvector(0,B[eDIM]->nglobal-1);/* intial divergence-free solution */
u = dvector(0,nsolve-1); /* Solution */
r = dvector(0,nsolve-1); /* residual */
z = dvector(0,nsolve-1); /* precondition solution */
if(eDIM == 2)
wk = dmatrix(0,1,0,eDIM*4*LGmax);
else
wk = dmatrix(0,1,0,eDIM*6*LGmax*LGmax);
nsol = nsolve;
}
if(B[eDIM]->nglobal > nglob)
{
if(nglob)
free(w);
w = dvector(0,B[eDIM]->nglobal-1); /* A*Search direction */
nglob = B[eDIM]->nglobal;
}
divergence_free_init(V,u0,p,B,wk);
dzero (B[eDIM]->nglobal, w, 1);
dzero (nsolve, u, 1);
dcopy (nsolve, p, 1, r, 1);
tolcg = dparam("TOLCG");
epsfac = 1.0;
eps = sqrt(ddot(nsolve,r,1,r,1))*epsfac;
if (option("verbose") > 1)
printf("\t %3d iterations, error = %#14.6g %lg %lg\n",
iter, eps/epsfac, epsfac, tolcg);
rtz = eps;
/* =========================================================== *
* ---- Conjugate Gradient Iteration ---- *
* =========================================================== */
while (eps > tolcg && iter++ < MAX_ITERATIONS )
{
/* while (sqrt(rtz) > tolcg && iter++ < MAX_ITERATIONS ){*/
Precon_Stokes(V[0],B[eDIM],r,z);
rtz = ddot (nsolve, r, 1, z, 1);
if (iter > 1)
{ /* Update search direction */
beta = rtz / rtz_old;
dsvtvp(nsolve, beta, p, 1, z, 1, p, 1);
}
else
dcopy(nsolve, z, 1, p, 1);
A_Stokes(V,B,p,w,wk);
alpha = rtz/ddot(nsolve, p, 1, w, 1);
daxpy(nsolve, alpha, p, 1, u, 1); /* Update solution... */
daxpy(nsolve,-alpha, w, 1, r, 1); /* ...and residual */
rtz_old = rtz;
eps = sqrt(ddot(nsolve, r, 1, r, 1))*epsfac; /* Compute new L2-error */
fprintf(stdout,"%d %lg %lg\n",iter,eps,sqrt(rtz));
}
/* =========================================================== *
* End of Loop *
* =========================================================== */
/* Save solution and clean up */
dcopy(nsolve,u,1,p,1);
/* add back u0 */
dvadd(nsolve,u0,1,p,1,p,1);
if (iter > MAX_ITERATIONS)
{
error_msg (Bsolve_Stokes_CG failed to converge);
}
else if (option("verbose") > 1)
printf("\t %3d iterations, error = %#14.6g %lg %lg\n",
iter, eps/epsfac, epsfac, tolcg);
return;
}
static void Bsolve_Stokes_PCR(Element_List **V, Bsystem **B, double *p)
{
int eDIM = V[0]->flist[0]->dim();
const int nsolve = B[eDIM]->nsolve;
const int nglobal = B[eDIM]->nglobal;
int iter = 0;
double tolcg, alpha, beta, eps, Aps, epsfac;
static double *u = (double*)0;
static double *s = (double*)0;
static double *r1 = (double*)0;
static double *r2 = (double*)0;
static double *Ap = (double*)0;
static double *Ar = (double*)0;
static double *z = (double*)0, **wk;
static int nsol = 0, nglob = 0;
if(nsolve > nsol)
{
if(nsol)
{
free(u);
free(s);
free(r1);
free(z);
free_dmatrix(wk,0,0);
}
/* Temporary arrays */
u = dvector(0,nsolve-1); /* Solution */
s = dvector(0,nsolve-1);
r1 = dvector(0,nsolve-1); /* residual */
z = dvector(0,nsolve-1); /* precondition solution */
if(eDIM == 2)
wk = dmatrix(0,1,0,eDIM*4*LGmax);
else
wk = dmatrix(0,1,0,eDIM*6*LGmax*LGmax);
nsol = nsolve;
}
if(nglobal > nglob)
{
if(nglob)
{
free(Ar);
free(Ap);
free(r2);
}
Ar = dvector(0,nglobal-1); /* A*r Search direction */
Ap = dvector(0,nglobal-1); /* A*p Search direction */
r2 = dvector(0,nglobal-1); /* residual */
nglob = nglobal;
}
dzero (B[eDIM]->nglobal, Ap, 1);
dzero (B[eDIM]->nglobal, Ar, 1);
dzero (B[eDIM]->nglobal, r2, 1);
dzero (nsolve, u, 1);
dcopy (nsolve, p, 1, r1, 1);
tolcg = dparam("TOLCG");
epsfac = 1.0;
eps = sqrt(ddot(nsolve,r1,1,r1,1))*epsfac;
if (option("verbose") > 1)
printf("\t %3d iterations, error = %#14.6g %lg %lg\n",
iter, eps/epsfac, epsfac, tolcg);
/* =========================================================== *
* ---- Conjugate Gradient Iteration ---- *
* =========================================================== */
while (eps > tolcg && iter++ < MAX_ITERATIONS )
{
if (iter > 1)
{ /* Update search direction */
A_Stokes(V,B,r2,Ar,wk);
beta = -ddot(nsolve,Ar,1,s,1)/Aps;
dsvtvp (nsolve,beta,p,1,r2,1,p,1);
dsvtvp (nsolve,beta,Ap,1,Ar,1,Ap,1);
}
else
{
Precon_Stokes(V[0],B[eDIM],r1,r2);
dcopy (nsolve, r2, 1, p, 1);
A_Stokes(V,B,p,Ap,wk);
}
Precon_Stokes(V[0],B[eDIM],Ap,s);
Aps = ddot(nsolve,s,1,Ap,1);
alpha = ddot(nsolve,r2,1,Ap,1)/Aps;
daxpy(nsolve, alpha, p , 1, u, 1); /* Update solution... */
daxpy(nsolve,-alpha, Ap, 1, r1, 1); /* ...and residual */
daxpy(nsolve,-alpha, s , 1, r2, 1); /* ...and residual */
eps = sqrt(ddot(nsolve, r1, 1, r1, 1))*epsfac; // Compute new L2-error
fprintf(stdout,"%d %lg %lg\n",iter,eps,sqrt(ddot(nsolve,r2,1,r2,1)));
}
/* =========================================================== *
* End of Loop *
* =========================================================== */
/* Save solution and clean up */
dcopy(nsolve,u,1,p,1);
if (iter > MAX_ITERATIONS)
{
error_msg (Bsolve_Stokes_CG failed to converge);
}
else if (option("verbose") > 1)
printf("\t %3d iterations, error = %#14.6g %lg %lg\n",
iter, eps/epsfac, epsfac, tolcg);
return;
}
static void divergence_free_init(Element_List **V, double *u0, double *r,
Bsystem **B, double **wk)
{
register int i,j,k,cnt;
int eDIM = V[0]->fhead->dim();
int asize,info,Nbmodes;
int **bmap = B[eDIM]->bmap;
int nel = B[eDIM]->nel;
int nglobal = B[eDIM]->nglobal;
int nsolve = B[eDIM]->nsolve;
int vsolve = B[0]->nsolve;
double **a = B[eDIM]->Gmat->a,*x,*b,*sc,*sc1;
static double *invb;
x = dvector(0,nel-1);
/* form and invert B' B system */
if(!invb)
{
invb = dvector(0,nel*(nel+1)/2-1);
sc = B[0]->signchange;
for(cnt = 0,k = 0; k < nel; ++k)
{
/* gather B from local systems */
Nbmodes = V[0]->flist[k]->Nbmodes;
asize = Nbmodes*eDIM+1;
b = a[V[0]->flist[k]->geom->id] + asize*(asize-1)/2;
dzero(nglobal,u0,1);
for(j = 0; j < eDIM; ++j)
{
for(i = 0; i < Nbmodes; ++i)
u0[bmap[k][i+j*Nbmodes]] += sc[i]*b[i+j*Nbmodes];
}
dzero(nglobal-nsolve,u0+nsolve,1);
/* take inner product with B' */
sc1 = B[0]->signchange;
for(j = k; j < nel; ++j,cnt++)
{
dzero(asize,wk[0],1);
dgathr(asize-1, u0, bmap[j], wk[0]);
for(i = 0; i < eDIM; ++i)
dvmul (Nbmodes,sc1,1,wk[0]+i*Nbmodes,1,wk[0]+i*Nbmodes,1);
Nbmodes = V[0]->flist[j]->Nbmodes;
asize = Nbmodes*eDIM+1;
b = a[V[0]->flist[j]->geom->id] + asize*(asize-1)/2;
invb[cnt] = ddot(asize-1,b,1,wk[0],1);
sc1 += Nbmodes;
}
sc += V[0]->flist[k]->Nbmodes;
}
/* take out first row to deal with singularity */
if(B[eDIM]->singular)
dzero(nel,invb,1);
invb[0] = 1.0;
dpptrf('L', nel, invb, info);
}
/* solve B' B x = r */
dcopy (nel,r+eDIM*vsolve,1,x,1);
dpptrs('L', nel, 1, invb, x, nel, info);
dzero(B[eDIM]->nglobal,u0,1);
/* generate initial vector as u0 = B x */
sc = B[0]->signchange;
for(k = 0; k < nel; ++k)
{
Nbmodes = V[0]->flist[k]->Nbmodes;
asize = Nbmodes*eDIM+1;
b = a[V[0]->flist[k]->geom->id] + asize*(asize-1)/2;
dsmul(asize-1,x[k],b,1,wk[0],1);
for(j = 0; j < eDIM; ++j)
{
for(i = 0; i < Nbmodes; ++i)
u0[bmap[k][i+j*Nbmodes]] += sc[i]*wk[0][i+j*Nbmodes];
}
sc += Nbmodes;
}
dzero(nglobal-eDIM*vsolve, u0 + eDIM*vsolve, 1);
/* subtract off a*u0 from r */
sc = B[0]->signchange;
for(k = 0; k < nel; ++k)
{
Nbmodes = V[0]->flist[k]->Nbmodes;
dzero(eDIM*Nbmodes+1,wk[0],1);
dgathr(eDIM*Nbmodes+1, u0, bmap[k], wk[0]);
for(j = 0; j < eDIM; ++j)
dvmul (Nbmodes, sc, 1, wk[0] + j*Nbmodes, 1, wk[0] + j*Nbmodes,1);
dspmv('U',eDIM*Nbmodes+1,1.0,a[V[0]->flist[k]->geom->id],
wk[0],1,0.0,wk[1],1);
for(j = 0; j < eDIM; ++j)
for(i = 0; i < Nbmodes; ++i)
r[bmap[k][i+j*Nbmodes]] -= sc[i]*wk[1][i+j*Nbmodes];
r[bmap[k][eDIM*Nbmodes]] -= wk[1][eDIM*Nbmodes];
sc += Nbmodes;
}
r[eDIM*vsolve] = 0.0;
dzero(nglobal-nsolve, r + nsolve, 1);
free(x);
}
static void setupRHS (Element_List **V, Element_List **Vf,double *rhs,
double *u0, Bndry **Vbc, Bsystem **Vbsys)
{
register int i,k;
int N,nbl;
int eDIM = V[0]->flist[0]->dim();
Bsystem *PB = Vbsys[eDIM],*B = Vbsys[0];
int nel = B->nel,info;
int **ipiv = B->Gmat->cipiv;
double **binvc = B->Gmat->binvc;
double **invc = B->Gmat->invc;
double ***dbinvc = B->Gmat->dbinvc;
double **p_binvc = PB->Gmat->binvc;
Element *E,*E1;
Bndry *Ebc;
double *tmp;
if(eDIM == 2)
tmp = dvector(0,max(8*LGmax,(LGmax-2)*(LGmax-2)));
else
tmp = dvector(0,18*LGmax*LGmax);
B = Vbsys[0];
PB = Vbsys[eDIM];
st1 = clock();
/* save initial condition */
saveinit(V,u0,Vbsys);
Timing1("saveinit..........");
/* take inner product if in physical space */
for(i = 0; i < eDIM; ++i)
{
if(Vf[i]->fhead->state == 'p')
Vf[i]->Iprod(Vf[i]);
}
/* zero pressure field */
dzero(Vf[eDIM]->hjtot,Vf[eDIM]->base_hj,1);
Timing1("zeroing...........");
/* condense out interior from u-vel + p */
for(i = 0; i < eDIM; ++i)
for(E=Vf[i]->fhead;E;E=E->next)
{
nbl = E->Nbmodes;
N = E->Nmodes - nbl;
if(N)
dgemv('T', N, nbl, -1., binvc[E->geom->id], N,
E->vert->hj+nbl, 1, 1., E->vert->hj,1);
}
Timing1("first condense(v).");
for(i = 0; i < eDIM; ++i)
for(E=Vf[i]->fhead;E;E=E->next)
{
nbl = E->Nbmodes;
N = E->Nmodes - nbl;
if(N)
{
E1 = Vf[eDIM]->flist[E->id];
if(B->lambda->wave)
{
dcopy(N,E->vert->hj+nbl,1,tmp,1);
dgetrs('N', N, 1, invc[E->geom->id], N,ipiv[E->geom->id],
tmp,N,info);
dgemv('T', N, E1->Nmodes, -1., dbinvc[i][E->geom->id], N,
tmp, 1, 1., E1->vert->hj,1);
}
else
{
dgemv('T', N, E1->Nmodes, -1., dbinvc[i][E->geom->id], N,
E->vert->hj+nbl, 1, 1., E1->vert->hj,1);
}
}
}
Timing1("first condense(p).");
/* add flux terms */
for(i = 0; i < eDIM; ++i)
for(Ebc = Vbc[i]; Ebc; Ebc = Ebc->next)
if(Ebc->type == 'F' || Ebc->type == 'R')
Vf[i]->flist[Ebc->elmt->id]->Add_flux_terms(Ebc);
/* second level of factorisation to orthogonalise basis to p */
for(E=Vf[eDIM]->fhead;E;E=E->next)
{
E1 = Vf[0]->flist[E->id];
nbl = eDIM*E1->Nbmodes + 1;
N = E->Nmodes-1;
dgemv('T', N, nbl, -1.0, p_binvc[E->geom->id], N,
E->vert->hj+1, 1, 0.0, tmp,1);
for(i = 0; i < eDIM; ++i)
{
E1 = Vf[i]->flist[E->id];
dvadd(E1->Nbmodes,tmp+i*E1->Nbmodes,1,E1->vert->hj,1,
E1->vert->hj,1);
}
E->vert->hj[0] += tmp[nbl-1];
}
Timing1("second condense...");
/* subtract boundary initial conditions */
if(PB->smeth == iterative)
{
double **wk;
double **a = PB->Gmat->a;
if(eDIM == 2)
wk = dmatrix(0,1,0,eDIM*4*LGmax);
else
wk = dmatrix(0,1,0,eDIM*6*LGmax*LGmax);
for(k = 0; k < nel; ++k)
{
nbl = V[0]->flist[k]->Nbmodes;
/* gather vector */
for(i = 0; i < eDIM; ++i)
dcopy(nbl,V[i]->flist[k]->vert->hj,1,wk[0]+i*nbl,1);
dspmv('U',eDIM*nbl+1,1.0,a[V[0]->flist[k]->geom->id],
wk[0],1,0.0,wk[1],1);
/* subtract of Vf */
for(i = 0; i < eDIM; ++i)
dvsub(nbl,Vf[i]->flist[k]->vert->hj,1,wk[1]+i*nbl,1,
Vf[i]->flist[k]->vert->hj,1);
Vf[eDIM]->flist[k]->vert->hj[0] -= wk[1][eDIM*nbl];
}
GathrBndry_Stokes(Vf,rhs,Vbsys);
free_dmatrix(wk,0,0);
}
else
{
if(Vbc[0]->DirRHS)
{
GathrBndry_Stokes(Vf,rhs,Vbsys);
/* subtract of bcs */
dvsub(PB->nsolve,rhs,1,Vbc[0]->DirRHS,1,rhs,1);
/* zero ic vector */
dzero(PB->nsolve,u0,1);
}
else
{
/* zero out interior components since only deal with boundary
initial conditions (interior is always direct) */
for(i = 0; i < eDIM; ++i)
for(E = V[i]->fhead; E; E = E->next)
{
nbl = E->Nbmodes;
N = E->Nmodes - nbl;
dzero(N, E->vert->hj + nbl, 1);
}
/* inner product of divergence for pressure forcing */
for(i = 0; i < eDIM; ++i)
V[i]->Trans(V[i], J_to_Q);
V[0]->Grad(V[eDIM],0,0,'x');
V[1]->Grad(0,Vf[eDIM],0,'y');
dvadd(V[1]->htot,V[eDIM]->base_h,1,Vf[eDIM]->base_h,1,
V[eDIM]->base_h,1);
if(eDIM == 3)
{
V[2]->Grad(0,V[eDIM],0,'z');
dvadd(V[2]->htot,V[eDIM]->base_h,1,Vf[eDIM]->base_h,1,
V[eDIM]->base_h,1);
}
#ifndef PCONTBASE
for(k = 0; k < nel; ++k)
V[eDIM]->flist[k]->Ofwd(*V[eDIM]->flist[k]->h,
V[eDIM]->flist[k]->vert->hj,
V[eDIM]->flist[k]->dgL);
#else
V[eDIM]->Iprod(V[eDIM]);
#endif
for(i = 0; i < eDIM; ++i)
{
for(k = 0; k < nel; ++k)
{
E = V[i]->flist[k];
nbl = E->Nbmodes;
N = E->Nmodes - nbl;
E->HelmHoltz(PB->lambda+k);
dscal(E->Nmodes, -B->lambda[k].d, E->vert->hj, 1);
if(N)
{
/* condense out interior terms in velocity */
dgemv('T', N, nbl, -1., binvc[E->geom->id], N,
E->vert->hj+nbl, 1, 1., E->vert->hj,1);
/* condense out interior terms in pressure*/
E1 = V[eDIM]->flist[k];
if(B->lambda->wave)
{
dcopy(N,E->vert->hj+nbl,1,tmp,1);
dgetrs('N',N,1,invc[E->geom->id],N,
ipiv[E->geom->id],tmp,N,info);
dgemv('T',N,E1->Nmodes,-1.,dbinvc[i][E->geom->id],N,
tmp, 1, 1., E1->vert->hj,1);
}
else
{
dgemv('T',N,E1->Nmodes,-1.,dbinvc[i][E->geom->id],N,
E->vert->hj+nbl, 1, 1., E1->vert->hj,1);
}
}
}
}
/* second level of factorisation to orthogonalise basis to p */
/* p - vel */
for(E=V[eDIM]->fhead;E;E=E->next)
{
E1 = V[0]->flist[E->id];
nbl = eDIM*E1->Nbmodes + 1;
N = E->Nmodes-1;
dgemv('T', N, nbl, -1.0, p_binvc[E->geom->id], N,
E->vert->hj+1, 1, 0.0, tmp,1);
for(i = 0; i < eDIM; ++i)
{
E1 = V[i]->flist[E->id];
dvadd(E1->Nbmodes,tmp+i*E1->Nbmodes,1,E1->vert->hj,1,
E1->vert->hj,1);
dvadd(E1->Nbmodes,E1->vert->hj,1,
Vf[i]->flist[E->id]->vert->hj,1,
Vf[i]->flist[E->id]->vert->hj,1);
}
Vf[eDIM]->flist[E->id]->vert->hj[0]+=E->vert->hj[0]+tmp[nbl-1];
}
Timing1("bc condense.......");
GathrBndry_Stokes(Vf,rhs,Vbsys);
Timing1("GatherBndry.......");
}
}
/* finally copy inner product of f into v for inner solve */
for(i = 0; i < eDIM; ++i)
for(E = V[i]->fhead; E; E= E->next)
{
nbl = E->Nbmodes;
N = E->Nmodes - nbl;
E1 = Vf[i]->flist[E->id];
dcopy(N, E1->vert->hj+nbl, 1, E->vert->hj+nbl, 1);
}
for(E = Vf[eDIM]->fhead; E; E = E->next)
{
E1 = V[eDIM]->flist[E->id];
dcopy(E->Nmodes,E->vert->hj,1,E1->vert->hj,1);
}
dzero(PB->nglobal-PB->nsolve, rhs + PB->nsolve, 1);
free(tmp);
}
int main (int argc,
char** argv)
/* ------------------------------------------------------------------------- *
* Wrapper.
* ------------------------------------------------------------------------- */
{
char buf[STR_MAX], fmt[STR_MAX];
int i, j, k, n, np, nzin, nzout, nel, nrep = 1, force = 0;
int nfields, nplane, nptin, nptout, ntot, swab;
FILE *fp_in = stdin, *fp_out = stdout;
double *datain, *dataout, beta;
getargs (argc, argv, &fp_in, &nrep, &force);
format (fmt);
while (fgets (buf, STR_MAX, fp_in)) {
fputs (buf, fp_out); fgets (buf, STR_MAX, fp_in);
fputs (buf, fp_out); fgets (buf, STR_MAX, fp_in);
if (sscanf (buf, "%d%*s%d%d", &np, &nzin, &nel) != 3)
message (prog, "unable to read the file size", ERROR);
if (!force) {
if ((nzout = roundup (nzin)) != nzin)
message (prog, "input nz does not have 2, 3, 5 factors", ERROR);
nzout = roundup (nzin * nrep);
} else
nzout = nzin * nrep;
fprintf (fp_out, hdr_fmt[2], np, np, nzout, nel);
n = 4;
while (n--) { fgets (buf, STR_MAX, fp_in); fputs (buf, fp_out); }
fgets (buf, STR_MAX, fp_in);
sscanf (buf, "%lf", &beta);
beta /= nrep;
fprintf (fp_out, hdr_fmt[7], beta);
fgets (buf, STR_MAX, fp_in); fputs (buf, fp_out);
for (nfields = 0, i = 0; i < 25; i++) if (isalnum(buf[i])) nfields++;
fgets (buf, STR_MAX, fp_in);
if (!strstr(buf, "binary"))
message (prog, "input file not binary format", ERROR);
swab = (strstr (buf, "big") && strstr (fmt, "little")) ||
(strstr (fmt, "big") && strstr (buf, "little"));
strcat (strcpy (buf, "binary "), fmt);
fprintf (fp_out, hdr_fmt[9], buf);
/* -- Set sizes, allocate storage. */
nplane = np * np * nel;
ntot = nplane + (nplane & 1);
nptin = nzin * ntot;
nptout = nzout * ntot;
datain = dvector (0, nptin - 1);
dataout = dvector (0, nptout - 1);
/* -- Read and write all data fields. */
for (i = 0; i < nfields; i++) {
dzero (nptin, datain, 1);
dzero (nptout, dataout, 1);
for (j = 0; j < nzin; j++) {
if (fread (datain+j*ntot, sizeof (double), nplane, fp_in) != nplane)
message (prog, "an error occured while reading", ERROR);
if (swab) dbrev (ntot, datain+j*ntot, 1, datain+j*ntot, 1);
}
if (force) { /* -- We can just copy in physical space. */
for (k = 0; k < nrep; k++) {
for (j = 0; j < nzin; j++) {
if (fwrite (datain+j*ntot, sizeof (double), nplane, fp_out) != nplane)
message (prog, "an error occured while writing", ERROR);
}
}
} else { /* -- Have to go to Fourier space for padding. */
dDFTr (datain, nzin, ntot, FORWARD);
pack (datain, nzin, dataout, nzout, nrep, ntot);
dDFTr (dataout, nzout, ntot, INVERSE);
for (j = 0; j < nzout; j++)
if (fwrite (dataout+j*ntot, sizeof (double), nplane, fp_out) != nplane)
message (prog, "an error occured while writing", ERROR);
}
}
}
freeDvector (datain, 0);
freeDvector (dataout, 0);
return EXIT_SUCCESS;
}
/**
Time domain physical simulation.
noisy:
- 0: no noise at all;
- 1: poisson and read out noise.
- 2: only poisson noise.
*/
dmat *skysim_sim(dmat **mresout, const dmat *mideal, const dmat *mideal_oa, double ngsol,
ASTER_S *aster, const POWFS_S *powfs,
const PARMS_S *parms, int idtratc, int noisy, int phystart){
int dtratc=0;
if(!parms->skyc.multirate){
dtratc=parms->skyc.dtrats->p[idtratc];
}
int hasphy;
if(phystart>-1 && phystart<aster->nstep){
hasphy=1;
}else{
hasphy=0;
}
const int nmod=mideal->nx;
dmat *res=dnew(6,1);/*Results. 1-2: NGS and TT modes.,
3-4:On axis NGS and TT modes,
4-6: On axis NGS and TT wihtout considering un-orthogonality.*/
dmat *mreal=NULL;/*modal correction at this step. */
dmat *merr=dnew(nmod,1);/*modal error */
dcell *merrm=dcellnew(1,1);dcell *pmerrm=NULL;
const int nstep=aster->nstep?aster->nstep:parms->maos.nstep;
dmat *mres=dnew(nmod,nstep);
dmat* rnefs=parms->skyc.rnefs;
dcell *zgradc=dcellnew3(aster->nwfs, 1, aster->ngs, 0);
dcell *gradout=dcellnew3(aster->nwfs, 1, aster->ngs, 0);
dmat *gradsave=0;
if(parms->skyc.dbg){
gradsave=dnew(aster->tsa*2,nstep);
}
SERVO_T *st2t=0;
kalman_t *kalman=0;
dcell *mpsol=0;
dmat *pgm=0;
dmat *dtrats=0;
int multirate=parms->skyc.multirate;
if(multirate){
kalman=aster->kalman[0];
dtrats=aster->dtrats;
}else{
if(parms->skyc.servo>0){
const double dtngs=parms->maos.dt*dtratc;
st2t=servo_new(merrm, NULL, 0, dtngs, aster->gain->p[idtratc]);
pgm=aster->pgm->p[idtratc];
}else{
kalman=aster->kalman[idtratc];
}
}
if(kalman){
kalman_init(kalman);
mpsol=dcellnew(aster->nwfs, 1); //for psol grad.
}
const long nwvl=parms->maos.nwvl;
dcell **psf=0, **mtche=0, **ints=0;
ccell *wvf=0,*wvfc=0, *otf=0;
if(hasphy){
psf=mycalloc(aster->nwfs,dcell*);
wvf=ccellnew(aster->nwfs,1);
wvfc=ccellnew(aster->nwfs,1);
mtche=mycalloc(aster->nwfs,dcell*);
ints=mycalloc(aster->nwfs,dcell*);
otf=ccellnew(aster->nwfs,1);
for(long iwfs=0; iwfs<aster->nwfs; iwfs++){
const int ipowfs=aster->wfs[iwfs].ipowfs;
const long ncomp=parms->maos.ncomp[ipowfs];
const long nsa=parms->maos.nsa[ipowfs];
wvf->p[iwfs]=cnew(ncomp,ncomp);
wvfc->p[iwfs]=NULL;
psf[iwfs]=dcellnew(nsa,nwvl);
//cfft2plan(wvf->p[iwfs], -1);
if(parms->skyc.multirate){
mtche[iwfs]=aster->wfs[iwfs].pistat->mtche[(int)aster->idtrats->p[iwfs]];
}else{
mtche[iwfs]=aster->wfs[iwfs].pistat->mtche[idtratc];
}
otf->p[iwfs]=cnew(ncomp,ncomp);
//cfft2plan(otf->p[iwfs],-1);
//cfft2plan(otf->p[iwfs],1);
ints[iwfs]=dcellnew(nsa,1);
int pixpsa=parms->skyc.pixpsa[ipowfs];
for(long isa=0; isa<nsa; isa++){
ints[iwfs]->p[isa]=dnew(pixpsa,pixpsa);
}
}
}
for(int irep=0; irep<parms->skyc.navg; irep++){
if(kalman){
kalman_init(kalman);
}else{
servo_reset(st2t);
}
dcellzero(zgradc);
dcellzero(gradout);
if(ints){
for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
dcellzero(ints[iwfs]);
}
}
for(int istep=0; istep<nstep; istep++){
memcpy(merr->p, PCOL(mideal,istep), nmod*sizeof(double));
dadd(&merr, 1, mreal, -1);/*form NGS mode error; */
memcpy(PCOL(mres,istep),merr->p,sizeof(double)*nmod);
if(mpsol){//collect averaged modes for PSOL.
for(long iwfs=0; iwfs<aster->nwfs; iwfs++){
dadd(&mpsol->p[iwfs], 1, mreal, 1);
}
}
pmerrm=0;
if(istep>=parms->skyc.evlstart){/*performance evaluation*/
double res_ngs=dwdot(merr->p,parms->maos.mcc,merr->p);
if(res_ngs>ngsol*100){
dfree(res); res=NULL;
break;
}
{
res->p[0]+=res_ngs;
res->p[1]+=dwdot2(merr->p,parms->maos.mcc_tt,merr->p);
double dot_oa=dwdot(merr->p, parms->maos.mcc_oa, merr->p);
double dot_res_ideal=dwdot(merr->p, parms->maos.mcc_oa, PCOL(mideal,istep));
double dot_res_oa=0;
for(int imod=0; imod<nmod; imod++){
dot_res_oa+=merr->p[imod]*IND(mideal_oa,imod,istep);
}
res->p[2]+=dot_oa-2*dot_res_ideal+2*dot_res_oa;
res->p[4]+=dot_oa;
}
{
double dot_oa_tt=dwdot2(merr->p, parms->maos.mcc_oa_tt, merr->p);
/*Notice that mcc_oa_tt2 is 2x5 marix. */
double dot_res_ideal_tt=dwdot(merr->p, parms->maos.mcc_oa_tt2, PCOL(mideal,istep));
double dot_res_oa_tt=0;
for(int imod=0; imod<2; imod++){
dot_res_oa_tt+=merr->p[imod]*IND(mideal_oa,imod,istep);
}
res->p[3]+=dot_oa_tt-2*dot_res_ideal_tt+2*dot_res_oa_tt;
res->p[5]+=dot_oa_tt;
}
}//if evl
if(istep<phystart || phystart<0){
/*Ztilt, noise free simulation for acquisition. */
dmm(&zgradc->m, 1, aster->gm, merr, "nn", 1);/*grad due to residual NGS mode. */
for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
const int ipowfs=aster->wfs[iwfs].ipowfs;
const long ng=parms->maos.nsa[ipowfs]*2;
for(long ig=0; ig<ng; ig++){
zgradc->p[iwfs]->p[ig]+=aster->wfs[iwfs].ztiltout->p[istep*ng+ig];
}
}
for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
int dtrati=(multirate?(int)dtrats->p[iwfs]:dtratc);
if((istep+1) % dtrati==0){
dadd(&gradout->p[iwfs], 0, zgradc->p[iwfs], 1./dtrati);
dzero(zgradc->p[iwfs]);
if(noisy){
int idtrati=(multirate?(int)aster->idtrats->p[iwfs]:idtratc);
dmat *nea=aster->wfs[iwfs].pistat->sanea->p[idtrati];
for(int i=0; i<nea->nx; i++){
gradout->p[iwfs]->p[i]+=nea->p[i]*randn(&aster->rand);
}
}
pmerrm=merrm;//record output.
}
}
}else{
/*Accumulate PSF intensities*/
for(long iwfs=0; iwfs<aster->nwfs; iwfs++){
const double thetax=aster->wfs[iwfs].thetax;
const double thetay=aster->wfs[iwfs].thetay;
const int ipowfs=aster->wfs[iwfs].ipowfs;
const long nsa=parms->maos.nsa[ipowfs];
ccell* wvfout=aster->wfs[iwfs].wvfout[istep];
for(long iwvl=0; iwvl<nwvl; iwvl++){
double wvl=parms->maos.wvl[iwvl];
for(long isa=0; isa<nsa; isa++){
ccp(&wvfc->p[iwfs], IND(wvfout,isa,iwvl));
/*Apply NGS mode error to PSF. */
ngsmod2wvf(wvfc->p[iwfs], wvl, merr, powfs+ipowfs, isa,
thetax, thetay, parms);
cembedc(wvf->p[iwfs],wvfc->p[iwfs],0,C_FULL);
cfft2(wvf->p[iwfs],-1);
/*peak in corner. */
cabs22d(&psf[iwfs]->p[isa+nsa*iwvl], 1., wvf->p[iwfs], 1.);
}/*isa */
}/*iwvl */
}/*iwfs */
/*Form detector image from accumulated PSFs*/
double igrad[2];
for(long iwfs=0; iwfs<aster->nwfs; iwfs++){
int dtrati=dtratc, idtrat=idtratc;
if(multirate){//multirate
idtrat=aster->idtrats->p[iwfs];
dtrati=dtrats->p[iwfs];
}
if((istep+1) % dtrati == 0){/*has output */
dcellzero(ints[iwfs]);
const int ipowfs=aster->wfs[iwfs].ipowfs;
const long nsa=parms->maos.nsa[ipowfs];
for(long isa=0; isa<nsa; isa++){
for(long iwvl=0; iwvl<nwvl; iwvl++){
double siglev=aster->wfs[iwfs].siglev->p[iwvl];
ccpd(&otf->p[iwfs],psf[iwfs]->p[isa+nsa*iwvl]);
cfft2i(otf->p[iwfs], 1); /*turn to OTF, peak in corner */
ccwm(otf->p[iwfs], powfs[ipowfs].dtf[iwvl].nominal);
cfft2(otf->p[iwfs], -1);
dspmulcreal(ints[iwfs]->p[isa]->p, powfs[ipowfs].dtf[iwvl].si,
otf->p[iwfs]->p, siglev);
}
/*Add noise and apply matched filter. */
#if _OPENMP >= 200805
#pragma omp critical
#endif
switch(noisy){
case 0:/*no noise at all. */
break;
case 1:/*both poisson and read out noise. */
{
double bkgrnd=aster->wfs[iwfs].bkgrnd*dtrati;
addnoise(ints[iwfs]->p[isa], &aster->rand, bkgrnd, bkgrnd, 0,0,IND(rnefs,idtrat,ipowfs));
}
break;
case 2:/*there is still poisson noise. */
addnoise(ints[iwfs]->p[isa], &aster->rand, 0, 0, 0,0,0);
break;
default:
error("Invalid noisy\n");
}
igrad[0]=0;
igrad[1]=0;
double pixtheta=parms->skyc.pixtheta[ipowfs];
if(parms->skyc.mtch){
dmulvec(igrad, mtche[iwfs]->p[isa], ints[iwfs]->p[isa]->p, 1);
}
if(!parms->skyc.mtch || fabs(igrad[0])>pixtheta || fabs(igrad[1])>pixtheta){
if(!parms->skyc.mtch){
warning2("fall back to cog\n");
}else{
warning_once("mtch is out of range\n");
}
dcog(igrad, ints[iwfs]->p[isa], 0, 0, 0, 3*IND(rnefs,idtrat,ipowfs), 0);
igrad[0]*=pixtheta;
igrad[1]*=pixtheta;
}
gradout->p[iwfs]->p[isa]=igrad[0];
gradout->p[iwfs]->p[isa+nsa]=igrad[1];
}/*isa */
pmerrm=merrm;
dcellzero(psf[iwfs]);/*reset accumulation.*/
}/*if iwfs has output*/
}/*for wfs*/
}/*if phystart */
//output to mreal after using it to ensure two cycle delay.
if(st2t){//Type I or II control.
if(st2t->mint->p[0]){//has output.
dcp(&mreal, st2t->mint->p[0]->p[0]);
}
}else{//LQG control
kalman_output(kalman, &mreal, 0, 1);
}
if(kalman){//LQG control
int indk=0;
//Form PSOL grads and obtain index to LQG M
for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
int dtrati=(multirate?(int)dtrats->p[iwfs]:dtratc);
if((istep+1) % dtrati==0){
indk|=1<<iwfs;
dmm(&gradout->p[iwfs], 1, aster->g->p[iwfs], mpsol->p[iwfs], "nn", 1./dtrati);
dzero(mpsol->p[iwfs]);
}
}
if(indk){
kalman_update(kalman, gradout->m, indk-1);
}
}else if(st2t){
if(pmerrm){
dmm(&merrm->p[0], 0, pgm, gradout->m, "nn", 1);
}
servo_filter(st2t, pmerrm);//do even if merrm is zero. to simulate additional latency
}
if(parms->skyc.dbg){
memcpy(PCOL(gradsave, istep), gradout->m->p, sizeof(double)*gradsave->nx);
}
}/*istep; */
}
if(parms->skyc.dbg){
int dtrati=(multirate?(int)dtrats->p[0]:dtratc);
writebin(gradsave,"%s/skysim_grads_aster%d_dtrat%d",dirsetup, aster->iaster,dtrati);
writebin(mres,"%s/skysim_sim_mres_aster%d_dtrat%d",dirsetup,aster->iaster,dtrati);
}
dfree(mreal);
dcellfree(mpsol);
dfree(merr);
dcellfree(merrm);
dcellfree(zgradc);
dcellfree(gradout);
dfree(gradsave);
if(hasphy){
dcellfreearr(psf, aster->nwfs);
dcellfreearr(ints, aster->nwfs);
ccellfree(wvf);
ccellfree(wvfc);
ccellfree(otf);
free(mtche);
}
servo_free(st2t);
/*dfree(mres); */
if(mresout) {
*mresout=mres;
}else{
dfree(mres);
}
dscale(res, 1./((nstep-parms->skyc.evlstart)*parms->skyc.navg));
return res;
}
int Build_M_K_R_SUPG(ParametersType *Parameters, MatrixDataType *MatrixData, FemStructsType *FemStructs, FemFunctionsType *FemFunctions)
{
int i, j, E, J1, J2, J3;
int neq, nel;
double kx, ky, Be_x, Be_y, gamma, TwoA, invArea, Area, norm, Be2_x, Be2_y, tau, h, h_shock, Eu;
double third=1.0/3.0, sixth = 1.0/6.0, twelfth= 1.0/12.0;
double y23, y31, y12, x32, x13, x21, X[3], Y[3], Beta[2], Ae[3][3], ke[3][3], me[3][3];
double ue[3], ueb, due[3], dueb, fe1, fe2, fe3, feb, Rpg11, Rpg21, Rpg31, Cpg12, Cpg13, Cpg23 , Csc12, Csc13, Csc23;
double D12, D13, D23, C11, C12, C13, R11, R12, XB, YB, *U, *dU;
double *R = FemStructs->F;
int **lm = FemStructs->lm;
NodeType *Node = FemStructs->Node;
ElementType *Element = FemStructs->Element;
neq = Parameters->neq;
nel = Parameters->nel;
kx = FemFunctions->Condutivity();
ky = FemFunctions->Condutivity();
gamma = FemFunctions->Reaction();
double alpha = Parameters->Alpha;
double dt = Parameters->DeltaT;
dzero(neq+1, R);
setzeros(Parameters,MatrixData);
U = (double*) mycalloc("U of 'Build_M_F_DD_Transiente'", Parameters->nnodes, sizeof(double));
dU = (double*) mycalloc("dU of 'Build_M_F_DD_Transiente'", Parameters->nnodes, sizeof(double));
eval_U_dU(Parameters,FemStructs,FemFunctions,U,dU);
for (E=0; E<nel; E++)
{
J1 = Element[E].Vertex[0];
J2 = Element[E].Vertex[1];
J3 = Element[E].Vertex[2];
X[0] = Node[J1].x;
X[1] = Node[J2].x;
X[2] = Node[J3].x;
Y[0] = Node[J1].y;
Y[1] = Node[J2].y;
Y[2] = Node[J3].y;
y23 = Y[1] - Y[2];
y31 = Y[2] - Y[0];
y12 = Y[0] - Y[1];
x32 = X[2] - X[1];
x13 = X[0] - X[2];
x21 = X[1] - X[0];
TwoA = x21*y31 - x13*y12;
Area = 0.5*TwoA;
invArea = 1.0/Area;
h = sqrt(fabs(TwoA));
XB = third*(X[0]+X[1]+X[2]);
YB = third*(Y[0]+Y[1]+Y[2]);
//Beta velocity calculation
FemFunctions->Velocity(XB, YB, Beta);
Be_x = Beta[0];
Be_y = Beta[1];
Be2_x = Be_x*Be_x;
Be2_y = Be_y*Be_y;
norm = sqrt(Be2_x + Be2_y);
if (norm>0)
tau = 0.5*h/norm;
else
tau = 0;
//Calculation of Font/////////
fe1 = FemFunctions->Font(X[0], Y[0], kx, gamma, Be_x, Be_y);
fe2 = FemFunctions->Font(X[1], Y[1], kx, gamma, Be_x, Be_y);
fe3 = FemFunctions->Font(X[2], Y[2], kx, gamma, Be_x, Be_y);
feb = third*(fe1+fe2+fe3);
//Calculation of u and du in element e
ue[0] = U[J1];
ue[1] = U[J2];
ue[2] = U[J3];
due[0] = dU[J1];
due[1] = dU[J2];
due[2] = dU[J3];
ueb = third * (ue[0] + ue[1] + ue[2]);
dueb = third * (due[0] + due[1] + due[2]);
// Galerkin diffusion matrix
D12 = 0.25*invArea*((kx * y23 * y31) + (ky * x32 * x13));
D13 = 0.25*invArea*((kx * y23 * y12) + (ky * x32 * x21));
D23 = 0.25*invArea*((kx * y31 * y12) + (ky * x13 * x21));
// Galerkin Convection matrix
C11 = sixth*(Be_x*y23 + Be_y*x32);
C12 = sixth*(Be_x*y31 + Be_y*x13);
C13 = sixth*(Be_x*y12 + Be_y*x21);
// Galerkin reaction matrix
R11 = sixth*(gamma*Area);
R12 = twelfth*(gamma * Area);
// SUPG convection matrix
Cpg12 = 0.25*invArea*tau * (y23*(Be_x*Be_x*y31 + Be_x*Be_y*x13) + x32*(Be_x*Be_y*y31 + Be_y*Be_y*x13));
Cpg13 = 0.25*invArea*tau * (y23*(Be_x*Be_x*y12 + Be_x*Be_y*x21) + x32*(Be_x*Be_y*y12 + Be_y*Be_y*x21));
Cpg23 = 0.25*invArea*tau * (y31*(Be_x*Be_x*y12 + Be_x*Be_y*x21) + x13*(Be_x*Be_y*y12 + Be_y*Be_y*x21));
// SUPG reaction matrix
Rpg11 = sixth*tau*gamma * (Be_x*y23 + Be_y*x32);
Rpg21 = sixth*tau*gamma * (Be_x*y31 + Be_y*x13);
Rpg31 = sixth*tau*gamma * (Be_x*y12 + Be_y*x21);
/***************** local length element **********************/
h_shock = FemFunctions->h_shock(Be_x, Be_y, ue[0], ue[1], ue[2], y23, y31, y12, x32, x13, x21, Area);
/**************************************************************/
/**************************** Shock capture parameter calculations***************************************/
Eu = FemFunctions->ShockCapture(kx, ky, Be_x, Be_y, gamma, ue[0], ue[1], ue[2], ueb, dueb, feb, y23, y31,y12, x32,x13,x21,invArea, h_shock);
/********************************************************************************************************/
// Shock capture correction matrix
Csc12 = Eu*0.25*invArea*(y23*y31 + x32*x13);
Csc13 = Eu*0.25*invArea*(y23*y12 + x32*x21);
Csc23 = Eu*0.25*invArea*(y31*y12 + x13*x21);
// Matrix Ae
me[0][0] = sixth*Area + tau*C11;
me[0][1] = twelfth*Area + tau*C11;
me[0][2] = twelfth*Area + tau*C11;
me[1][0] = twelfth*Area + tau*C12;
me[1][1] = sixth*Area + tau*C12;
me[1][2] = twelfth*Area + tau*C12;
me[2][0] = twelfth*Area + tau*C13;
me[2][1] = twelfth*Area + tau*C13;
me[2][2] = sixth*Area + tau*C13;
ke[0][0] = -(D12 + D13) + C11 - (Cpg12 + Cpg13) + R11 + Rpg11 - (Csc12 + Csc13);
ke[0][1] = D12 + C12 + Cpg12 + R12 + Rpg11 + Csc12;
ke[0][2] = D13 + C13 + Cpg13 + R12 + Rpg11 + Csc13;
ke[1][0] = D12 + C11 + Cpg12 + R12 + Rpg21 + Csc12;
ke[1][1] = -(D12 + D23) + C12 - (Cpg12 + Cpg23) + R11 + Rpg21 - (Csc12 + Csc23);
ke[1][2] = D23 + C13 + Cpg23 + R12 + Rpg21 + Csc23;
ke[2][0] = D13 + C11 + Cpg13 + R12 + Rpg31 + Csc13;
ke[2][1] = D23 + C12 + Cpg23 + R12 + Rpg31 + Csc23;
ke[2][2] = -(D13 + D23) + C13 - (Cpg13 + Cpg23) + R11 + Rpg31 - (Csc13 + Csc23);
for (i=0; i<3; i++)
for (j=0; j<3; j++)
Ae[i][j] = me[i][j] + alpha*dt*ke[i][j];
//Calculation of vector R
R[lm[E][0]] += Area*twelfth*(2*fe1 + fe2 + fe3 ) + sixth*tau*(fe1+fe2+fe3)*(Be_x*y23 + Be_y*x32);
R[lm[E][1]] += Area*twelfth*( fe1 + 2*fe2 + fe3 ) + sixth*tau*(fe1+fe2+fe3)*(Be_x*y31 + Be_y*x13);
R[lm[E][2]] += Area*twelfth*( fe1 + fe2 + 2*fe3) + sixth*tau*(fe1+fe2+fe3)*(Be_x*y12 + Be_y*x21);
int I;
for (i=0; i<3; i++){
I = lm[E][i];
for (j=0; j<3; j++)
R[I] -= me[i][j]*due[j] + ke[i][j]*ue[j];
}
R[neq] = 0;
// Matrix assembly according to chosen storage scheme (EBE, EDE or CSR)
FemFunctions->assembly(Parameters, MatrixData, FemStructs, E, Ae);
}
free(U);
free(dU);
return 0;
}
void Hex::PutFace(double *from, int fac){
int i;
if(from){
switch(fac){
case 0:
dcopy(qa*qb, from ,1 ,**h_3d ,1);
break;
case 1:
for(i = 0; i < qc; ++i)
dcopy(qa, from + i*qa, 1, **h_3d + i*qa*qb, 1);
break;
case 2:
for(i = 0; i < qc; ++i)
dcopy(qb, from+i*qb, 1, **h_3d + qa-1 + i*qa*qb, qa);
break;
case 3:
for(i = 0; i < qc; ++i)
dcopy(qa, from+i*qa, 1, **h_3d + qa*(qb-1) + i*qa*qb, 1);
break;
case 4:
for(i = 0; i < qc; ++i)
dcopy(qb, from + i*qb, 1, **h_3d + i*qa*qb, qa);
break;
case 5:
dcopy(qa*qb, from ,1 ,**h_3d + (qc-1)*qa*qb ,1);
break;
default:
error_msg(GetFace -- unknown face);
break;
}
}
else{
switch(fac){
case 0:
dzero(qa*qb, **h_3d ,1);
break;
case 1:
for(i = 0; i < qc; ++i)
dzero(qa, **h_3d + i*qa*qb, 1);
break;
case 2:
for(i = 0; i < qc; ++i)
dzero(qb, **h_3d + qa-1 + i*qa*qb, qa);
break;
case 3:
for(i = 0; i < qc; ++i)
dzero(qa, **h_3d + qa*(qb-1) + i*qa*qb, 1);
break;
case 4:
for(i = 0; i < qc; ++i)
dzero(qb, **h_3d + i*qa*qb, qa);
break;
case 5:
dzero(qa*qb, **h_3d + (qc-1)*qa*qb ,1);
break;
default:
error_msg(GetFace -- unknown face);
break;
}
}
}
int main()
/* ========================================================================= *
* Driver routine.
* ========================================================================= */
{
double *c, *z, *y, *w, **D, **DT;
double x, xcheb, PI_n, poly, pder;
int i, j, n, np;
c = dvector(0, VEC_MAX-1);
z = dvector(0, VEC_MAX-1);
y = dvector(0, VEC_MAX-1);
w = dvector(0, VEC_MAX-1);
D = dmatrix(0, VEC_MAX-1, 0, VEC_MAX-1);
DT = dmatrix(0, VEC_MAX-1, 0, VEC_MAX-1);
/* ----------------------------------------------------------------------- *
* Test routine that finds Gauss-Lobatto-Jacobi points (and, indirectly, *
* the one that evaluates Jacobi polynomials) by finding the Gauss- *
* Lobatto-Chebyshev points, which are available in closed form. *
* Routine is from Canuto, App. C. *
* ----------------------------------------------------------------------- */
printf("\n---test value--- TEST JACGL ---true value---\n");
for (i=2; i<=6; i++) {
n = i << 1;
np = n + 1;
PI_n = M_PI / (double) n;
jacgl(n, -0.5, -0.5, z);
printf("\nChebyshev Points, N=%1d\n", n);
for (j=0; j<np; j++) {
xcheb = -cos(j*PI_n);
printf("%20.17g\t%20.17g\n", z[j], xcheb);
}
}
/* ----------------------------------------------------------------------- *
* Test the Gauss-Legendre points and weights: we have some closed-form *
* results available, e.g. Hughes \S 3.8. We will just try a 3-point rule. *
* ----------------------------------------------------------------------- */
printf("\n---test value--- TEST ZWGL ---true value---\n");
zwgl(z, w, 3);
printf("\nPoints for NP=3\n");
printf("%20.17g\t%20.17g\n", z[0], -sqrt(3.0/5.0));
printf("%20.17g\t%20.17g\n", z[1], 0.0);
printf("%20.17g\t%20.17g\n", z[2], sqrt(3.0/5.0));
printf("\nWeights for NP=3\n");
printf("%20.17g\t%20.17g\n", w[0], 5.0/9.0);
printf("%20.17g\t%20.17g\n", w[1], 8.0/9.0);
printf("%20.17g\t%20.17g\n", w[2], 5.0/9.0);
/* ----------------------------------------------------------------------- *
* Test the production of derivative-operator matrices by comparing dgll, *
* dermat_k and dermat_g (which should return the same results, barring *
* rounding errors) to "analytic" derivative obtained by extracting the *
* coefficients of the interpolating polynomials & differentiating by *
* synthetic division. *
* ----------------------------------------------------------------------- */
printf("\nTESTING DERIVATIVE OPERATORS\n");
jacgl(4, 0.0, 0.0, z);
printf("*** analytic (5x5):\n");
for (i=0; i<5; i++) {
dzero(5, w, 1);
w[i] = 1.0;
polcoe(4, z, w, c);
for (j=0; j<5; j++)
polder(4, c, z[j], &poly, DT[j] + i);
}
for (i=0; i<5; i++) {
for (j=0; j<5; j++)
printf("%23.17g", DT[i][j]);
printf("\n");
}
printf("*** dgll (5x5):\n");
dgll(5, z, D, DT);
for (i=0; i<5; i++) {
for (j=0; j<5; j++)
printf("%23.17g", D[i][j]);
printf("\n");
}
printf("*** dermat_k (5x5):\n");
dermat_k(5, z, D, DT);
for (i=0; i<5; i++) {
for (j=0; j<5; j++)
printf("%23.17g", D[i][j]);
printf("\n");
}
printf("*** dermat_g (5x5):\n");
dermat_g(5, z, 5, z, D, DT);
for (i=0; i<5; i++) {
for (j=0; j<5; j++)
printf("%23.17g", D[i][j]);
printf("\n");
}
/* ----------------------------------------------------------------------- *
* Test intmat_g by comparing results to "analytic" evaluations. *
* Start with 5 points evenly-spaced on [-1, 1], interpolate to 4 internal *
* Gauss-Legendre points. *
* ----------------------------------------------------------------------- */
printf("\nTESTING INTERPOLATION OPERATOR\n");
z[0] = -(z[4] = 1.0);
z[1] = -(z[3] = 0.5);
z[2] = 0.0;
jacg(3, 0.0, 0.0, w);
printf("*** analytic: (4x5)\n");
for (i=0; i<5; i++) { /* 5 Lagrange interpolants. */
dzero(5, y, 1);
y[i] = 1.0;
polcoe(4, z, y, c);
for (j=0; j<4; j++) /* 4 points to interpolate at. */
polder(4, c, w[j], DT[j]+i, &pder);
}
for (i=0; i<4; i++) {
for (j=0; j<5; j++)
printf("%23.17g", DT[i][j]);
printf("\n");
}
printf("*** intmat_g (4x5):\n");
intmat_g(5, z, 4, w, D, DT);
for (i=0; i<4; i++) {
for (j=0; j<5; j++)
printf("%23.17g", D[i][j]);
printf("\n");
}
}
