void newton( double *u, int n, double *f, double *v)
{
/* linearized newtons method for nonlinear pde
* see p. 149 trottenberg for example */
int i,j,nmem;
double h,h2,d2;
double nonlin,denom,*v2;
int aux;
double predenom;
h=(xmax-xmin)/(double) n;
//this is h : h= (xmax-xmin)/n, assuming xmax/min=ymax/min
h2=h*h; // if you change this, you must also change d2 in defect !!!
d2=1./h2;
// printf("Loop from 0 0 to i:%d j:%d\n", i, j );
predenom = -4.*d2 +invtau;
for(i=0;i<n;i++){
for(j=0;j<n;j++){
aux =index2d(i,j,n);
denom= predenom + df(u[aux]);
// printf("-4.* %f + %f\n", d2, invtau );
v[aux]=
(f[aux]-(v[index2d(i-1,j,n)]+v[index2d(i,j-1,n)]
+ v[index2d(i+1,j,n)]+v[index2d(i,j+1,n)] )*d2 )/denom;
}
}
}/*newton*/
void get_path_ofs(TPath *path, const TCoords& start_p, const TCoords& finish_p)
{
size_t ci = index2d(finish_p.x, finish_p.y);
size_t si = index2d(start_p.x, start_p.y);
while (ci != si)
{
TCoords p;
p.x = attrs[ci].ofsx;
p.y = attrs[ci].ofsy;
path->push_back(p);
ci -= W * p.y + p.x;
}
}
double errordefect( double *d, int n) // calculates one norm of the
// matrix d
{
int i,j;
double diff;
diff=0.;
for(i=0; i<n; i++){
for(j=0; j<n; j++){
diff += d[index2d(i,j,n)]*d[index2d(i,j,n)];
}
}
return(diff);
}/* errordefect */
void interpolate(double *uc,double *uf,int nc,int nf)
{
int i1,i,j,j1,n;
int ic,jc;
/* first equate the matching points on grid */
n=nc;
for( ic=0;ic<nc;ic++){ /* remember uc,nc is coarse grid, uf nf is fine */
for( jc=0;jc<nc;jc++){
i1=ic*2;
j1=jc*2;
uf[index2d(i1,j1,nf)]=uc[index2d(ic,jc,nc)]; /* for uf this is even i, even j */
}//j1
}//i1
/* now match up i pts between defined from previous pts - these are 'odd i', even j */
n=nf;
for( i=0;i<n;i+=2){ /* i should increment by 2 */
for( j=1;j<n;j+=2){
uf[index2d(i,j,nf)]=(uf[index2d(i,j-1,nf)] + uf[index2d(i,j+1,nf)])*0.5;
}//j
}//i
for( i=1;i<n;i+=2){
for( j=0;j<n;j++){
uf[index2d(i,j,nf)]=(uf[index2d(i-1,j,nf)] + uf[index2d(i+1,j,nf)])*0.5; /** note that i,j has been reversed **/
}//i
}//j
}/* interpolate */
void mexFunction(int lhsCount, mxArray* lhs[], int rhsCount, const mxArray* rhs[])
{
mxArray* pi_R;
unsigned int* pi_R_data;
unsigned int* pi_R_j_data;
unsigned int pi_R_j_idx;
unsigned int* inv_pi = (unsigned int*)mxGetData(rhs[0]);
size_t max_t = mxGetM(rhs[0]);
size_t N = mxGetN(rhs[0]);
unsigned int* t = (unsigned int*)mxGetData(rhs[1]);
unsigned int n = (unsigned int)mxGetScalar(rhs[2]);
char* already_ranked = mxMalloc(n * sizeof(char));
unsigned int* this_inv_pi;
unsigned int i, j, k;
unsigned int row;
mwSize dims[3];
dims[0] = n - 1;
dims[1] = max_t;
dims[2] = N;
pi_R = mxCreateNumericArray(3, dims, mxUINT32_CLASS, mxREAL);
pi_R_data = (unsigned int*)mxGetData(pi_R);
lhs[0] = pi_R;
for (i = 0; i < N; ++i)
{
this_inv_pi = inv_pi + index2d(0, max_t, i);
memset(already_ranked, 0, n * sizeof(char));
for (j = 0; j < t[i]; ++j)
{
/* Get this sparse vector of R */
pi_R_j_data = pi_R_data + index3d(0, n - 1, j, max_t, i);
pi_R_j_idx = 0;
/* Fill vector with proper entries */
/* Only row will be inv_pi[j] */
row = this_inv_pi[j];
already_ranked[row] = 1;
for (k = 0; k < n; ++k)
{
/* If not already ranked, and not current item being ranked, mark in vector */
if (already_ranked[k] == 0)
{
pi_R_j_data[pi_R_j_idx++] = k * n + row;
}
}
assert(pi_R_j_idx == n - j - 1);
}
}
mxFree(already_ranked);
}
void funct(double *f,double *u,int n, int t)
{
int i,j,k;
double x,y,z,h,t1;
double d2,sxsy,sint,sint1,uij,fij,fm1,fm,cost,cost1;
t1=(t-1)*tau;
d2=(double)n/(xmax-xmin);
d2=d2*d2;
h=(xmax-xmin)/(double) n;
for(i=0;i<n;i++){
x=i*h +xmin;
for(j=0;j<n;j++){
y=j*h +xmin;
sxsy=sin(x)*sin(y); // definitions to make things easier
sint=sin(t*tau);
sint1=sin(t1);
cost=cos(t*tau);
cost1=cos(t1);
uij=u[index2d(i,j,n)];
// define the functions f^m+1, f^m here to makes things easier
// eqn is : Nu +f = d_t u => f=d_t u-Nu
// need f at this time (t), and previous time (t-1)
// see comments at begining for more explanation
fm = sxsy*cost + 2*sxsy*sint - fn(sxsy*sint);
fm1 = sxsy*cost1 + 2*sxsy*sint1 - fn(sxsy*sint1);
fij=(u[index2d(i+1,j,n)]+u[index2d(i-1,j,n)]+
u[index2d(i,j+1,n)]+u[index2d(i,j-1,n)] -4.*uij)*d2;
fij+=uij*(2./tau) + fn(uij) + fm1 +fm;
f[index2d(i,j,n)]=-fij;
}//j
}//i
} /* funct */
void defectnl(double *d, double *u,int n,double *f)
{
int i,j;
double h,h2,d2,temp,temp1,temp2,temp3,b1;
h=(xmax-xmin)/(double) n; //this is h : h= (xmax-xmin)/n
h2=h*h;
d2=1./h2;
for( i=0;i<n;i++){
for( j=0;j<n;j++){ // d = f - Nu
d[index2d(i,j,n)]=f[index2d(i,j,n)] -
+ ( (u[index2d(i-1,j,n)]+u[index2d(i,j-1,n)]
+u[index2d(i+1,j,n)]+u[index2d(i,j+1,n)] -4*u[index2d(i,j,n)])*d2
+invtau*u[index2d(i,j,n)]
+fn(u[index2d(i,j,n)]) );
}
}
}/* defectnl */
double error(double *u,double *uback, int n,int *imx,int *jmx)
{ double diff,diffmx,ave;
int i,j,ic,k; /* calculates the difference between u now &
u before, as one way to calculate error */
ave=0;
diffmx=0;
ic=0;
for(i=0;i<n;i++){
for(j=0;j<n;j++){
diff=u[index2d(i,j,n)]-uback[index2d(i,j,n)];
if(diff < 0) diff=-diff;
ave+=diff;
ic+=1;
if(diff > diffmx){
diffmx=diff;
*imx=i;
*jmx=j;
} //if
}//j
}//i
ave=ave/ic;
return (diffmx);
}/*error*/
void ic(double *u,int n,int t) // initial condition
{
int i,j;
double x,y,h,timebefore;
h=(xmax-xmin)/(double) n;
timebefore=(double)(t-1)*tau;
for(i=0;i<n;i++){
x=i*h+xmin;
for(j=0;j<n;j++){
y=j*h+xmin;
u[index2d(i,j,n)]=sin(x)*sin(y)*sin(timebefore);
}//i
}//j
}/* ic */
void restrct2(double *uc, double *uf, int n)
{
int i, j, i2, j2,nf;
nf=2*n;
for( i=0;i<n;i++){ /* n is the # of pts on the coarse grid */
for( j=0;j<n;j++) {
i2=2*i;
j2=2*j;
uc[index2d(i,j,n)]=uf[index2d(i2,j2,nf)]*4.
+uf[index2d(i2,j2+1,nf)] +uf[index2d(i2+1,j2,nf)]
+uf[index2d(i2-1,j2,nf)]+uf[index2d(i2,j2-1,nf)];
uc[index2d(i,j,n)]=uc[index2d(i,j,n)]/8.;
}/* loop over j */
}/* loop over i */
} /* end restrct2 */
bool do_search(const TMap& map, const TCoords& start_p, const TCoords& finish_p)
{
static struct { int x, y, d; } dirs[] =
{ { -1, -1, 14 },{ 0, -1, 10 },{ 1, -1, 14 },{ -1, 0, 10 },{ 1, 0, 10 },{ -1, 1, 14 },{ 0, 1, 10 },{ 1, 1, 14 } };
memset(attrs, 0, sizeof(Attributes) * H * W);
while (!opened.empty()) opened.pop();
AttrsPtr current = opened_push(start_p, cost_estimate(start_p, finish_p));
while (!opened.empty())
{
current = opened_pop();
if (current.pos.x == finish_p.x && current.pos.y == finish_p.y)
return true;
for (int i = 0; i < 8; ++i)
{
int dx = dirs[i].x;
int dy = dirs[i].y;
TCoords npos;
npos.x = current.pos.x + dx;
npos.y = current.pos.y + dy;
if (!inbound(npos.x, npos.y) || map.isobstacle(npos.x, npos.y))
continue;
size_t ni = index2d(npos.x, npos.y);
if (attrs[ni].state == st_Closed)
continue;
TWeight t_gscore = current.pa->gscore + dirs[i].d;
if (attrs[ni].state == st_Wild)
{
opened_push(npos, t_gscore + cost_estimate(npos, finish_p));
}
else
{
if (t_gscore >= attrs[ni].gscore)
continue;
rearrange(&attrs[ni], t_gscore + cost_estimate(npos, finish_p));
}
attrs[ni].ofsx = dx;
attrs[ni].ofsy = dy;
attrs[ni].gscore = t_gscore;
}
}
return false;
}
void newtonnl( double *u, int n, double *f)
{ //newtons method directly for pde
int i,j,nmem; double h,h2,d2,*u2;
double nonlin,denom;
h=(xmax-xmin)/(double) n;
h2=h*h; // if you change this, you must also change d2 in defect !!!
d2=1./h2;
for(i=0;i<n;i++){
for(j=0;j<n;j++){
denom= -4.*d2 +invtau+df(u[index2d(i,j,n)]);
nonlin=u[index2d(i,j,n)]*invtau +fn(u[index2d(i,j,n)]) -f[index2d(i,j,n)];
u[index2d(i,j,n)]=
u[index2d(i,j,n)]
-( (u[index2d(i-1,j,n)]+u[index2d(i,j-1,n)]+ u[index2d(i+1,j,n)]
+u[index2d(i,j+1,n)] -4*u[index2d(i,j,n)])*d2 +nonlin )/denom;
}//j
}//i
}/*newtonnl*/
void restrct(double *uc, double *uf, int n)
{
int i, j, i2, j2,nf;
nf=2*n;
/*first equate the matching points on grid */
/*uc=coarse, uf=fine */
for( i=0;i<n;i++){ // n is the # of pts on the coarse grid
for( j=0;j<n;j++) {
i2=2*i;
j2=2*j;
uc[index2d(i,j,n)]=uf[index2d(i2,j2,nf)] *4.
+2.*uf[index2d(i2,j2+1,nf)] +2.*uf[index2d(i2+1,j2,nf)]
+2.*uf[index2d(i2-1,j2,nf)]+2.*uf[index2d(i2,j2-1,nf)]
+uf[index2d(i2-1,j2+1,nf)] +uf[index2d(i2+1,j2-1,nf)]
+uf[index2d(i2-1,j2-1,nf)] +uf[index2d(i2+1,j2+1,nf)];
uc[index2d(i,j,n)]=uc[index2d(i,j,n)]/16.;
// alternate schemes:
// uc[index2d(i,j,n)]=uf[index2d(i2,j2,nf)] ;
// or
// uc[index2d(i,j,n)]=uf[index2d(i2,j2,nf)] *4.
// +uf[index2d(i2,j2+1,nf)] +uf[index2d(i2+1,j2,nf)]
// +uf[index2d(i2-1,j2,nf)]+uf[index2d(i2,j2-1,nf)]
// uc[index2d(i,j,n)]=uc[index2d(i,j,n)]/8;
}//j
}//i
} /* end restrct */
void mgrid(double *u, int n,double *f,int t)
{
double *v,*d,*vc,*dc, *uf, *uc,*fc;
double *uback,*uback2;
double diffmx,diff,difv,x,y,tol,h;
double dif,mxd,sumu,sumu2,sum3,sol;
int i,j,k,imx,jmx,in,nc,irep;
int ncmem,nsq,nmem;
char filename[filesize];
FILE *f1, *f2, *out, *f3, *f8;
tol=10.e-12; // tol= tolerance. How accurately you want to solve the eqn
diffmx=tol*5.;
irep=0;
nc=n/2;
nmem=(n+1)*(n+1); // bigger than [0:n]*[0:n], just to ensure enough space
ncmem=(nc+1)*(nc+1);// same for the grid that is half-size
nsq=n*n;
/* --- requesting space for variables ------------------------------*/
v=malloc(nmem*sizeof(double));
if(v==NULL) {printf(" oopsies can't allocate v %d\n",__LINE__);}
d=malloc(nmem*sizeof(double));
if(d==NULL) printf(" oopsies can't allocate d %d\n",__LINE__);
uback=malloc(nmem*sizeof(double));
if(uback==NULL) printf(" oopsies can't allocate uback %d\n",__LINE__);
uback2=malloc(nmem*sizeof(double));
if(uback2==NULL) printf(" oopsies can't allocate uback2 %d\n",__LINE__);
vc=malloc(ncmem*sizeof(double));
if(vc==NULL) {printf(" oopsies can't allocate vc %d\n",__LINE__);}
uc=malloc(ncmem*sizeof(double));
if(uc==NULL) {printf(" oopsies can't allocate uc %d\n",__LINE__);}
dc=malloc(ncmem*sizeof(double));
if(dc==NULL) printf(" oopsies can't allocate dc %d\n",__LINE__);
for(i=0;i<nmem;i++){ // initialize to 0
v[i]=0.;
uback[i]=0.;
d[i]=0.; } // end of v,d,uback initialization
for(i=0;i<ncmem;i++){
vc[i]=0.;
dc[i]=0.; } // end of vc,dc
h=(xmax-xmin)/(double) n;
for(i=0;i<n;i++){
x=xmin+h*i;
for(j=0;j<n;j++){
y=xmin+h*j;
uback2[index2d(i,j,n)]=sin(x)*sin(y)*sin(t*tau);
}//y // the actual solution, to compare
}//x
diffmx=5*tol; /* tol determines solution tolerance. diffmx will measure
that difference */
irep=0; // irep will be the number of MG steps required to solve
/* +++++++++++++++ start of while loop ++++++++++++++++++*/
while(diffmx>tol){
diffmx=0.; // need to zero this now, will measure later
irep=irep+1; // counts number of MG steps
for( in=1;in<=nrep1;in++){ // presmoothing
newtonnl(u,n,f);
}//nrep1
for( i=0;i<nmem;i++){
uback[i]=u[i];
}//uback=u
/* defectnl calculates the residual. ie, given Nu=f
* d=f-Nu, or a measure of how far away you are from the solution */
defectnl(d,u,n,f);
/* restrict u, have a separate routine for it, in case you want to
* use a different restriction for than for other variables */
restrct2(uc,u,nc);
/* v is the linearlized version of u
* given, Nu=f
* define an operator K such that Kv=d,
* then u := u+v (ie new u = old u+ v, v found from above
* this can be derived from standard Newton's method:
* u := u - (Nu -f)/K
* where K=(d/du)N
*
* so, if Nu = (\nabla)^2 u + g(u)
* K = (\nabla)^2 + (d/du)g
*
* thus, the eqn we now solve is
*
* Kv = (\nabla)^2v + (d/du)g v =d (#)
*
* and the next iterate of u is u+v
*
* I apply MG techniques to (#)
*/
restrct(vc,v,nc);
restrct(dc,d,nc); // dc is defect on coarse grid
pooofle(vc,nc,dc,uc); /* funky multigrid part!
this solves for vc */
interpolate(vc,v,nc,n);//interpolate vc up to the fine grid
for(i=1;i<2;i++){
newton(u,n,d,v); // this is 'postsmoothing' for v !!
}
for( i=0;i<nmem;i++){
u[i]+=v[i];
uback[i]=u[i];
}//u+=v
for( i=0;i<nrep2;i++){
newtonnl(u,n,f); // postsmoothing on u
}//nrep2
/* how to tell when you're done?
* the classic way is to calculate the defect, and when the norm
* of the defect is less than some predetermined accuracy (tolerance)
* you're done.
*
* I have found that I get better accuracy - ie the solution remains
* correct farther into time when I use as my condition the difference
* between the current u, and the previous value of u. If anyone has
* any ideas for this, please let me know !
*/
defectnl(d,u,n,f);
diff=errordefect(d,n)/n/n;
diffmx=error(u,uback,n,&imx,&jmx);
/* diffmx is the condition to test against. for termination of
* calculation based on defect, do:
* diffmx=errordefect(d,n)/n/n;
* diff=error(u,uback,n,&imx,&jmx);
*/
mxd=-10.;
sum3=0.;
for(i=0;i<n;i++){
for(j=0;j<n;j++){
dif=uback2[index2d(i,j,n)]-u[index2d(i,j,n)];
sum3+=u[index2d(i,j,n)];
if(dif < 0.e-10) dif= -dif;
if(dif > mxd) {mxd= dif; // maximum difference between actual &
imx=i; // calculated solution. for fun
jmx=j; // imx & jmx calculate where this happens
} // end if
}//j
}//i
if(irep>2000) break; // some break condition
} /* while */
printf("%d %d %1.7e %1.7e %1.7e %d %d \n",t,irep,diffmx,diff,mxd,imx,jmx);
fflush(stdout);
/*
* this will make a file with the x,y coordinates, the calculated &
* actual solutions & their difference.
*
* uncomment if you want this printed out
*
snprintf(filename,filesize,"solution.%d",t);
f1=fopen(filename,"w");
for(i=0;i<n;i++){
x=xmin+h*i;
for(j=0;j<n;j++){
y=xmin+h*j;
sol=sin(x)*sin(y)*sin(t*tau);
dif=sol-u[index2d(i,j,n)];
fprintf(f1,"%lf %lf %lf %lf %lf\n",x,y,u[index2d(i,j,n)],sol,dif,uback[index2d(i,j,n)]);
}
}
fclose(f1);
*/
free(uback);
free(uback2);
free(v);
free(d);
free(dc);
free(uc);
free(vc);
} /* mg */
main(int argc, char *argv[])
{
double *areal; /* areal is the variable I'm solving for. will be called u
in the functions that are dependent on this on. I just
needed to give it a different name here */
double *freal; /* freal is the right hand side of (**), ie F */
double *f,x,y,h,dif,sol;
int n,nmem,t,i,j;
FILE *f1 ;
char filename[filesize];
imin = imax = jmin = jmax =stridemax;
stridemin = 0;
/* Use command line argument if it exists. Otherwise use default value in
nmax. */
n = (argc > 1) ? atoi(argv[1]) : nmax;
//n=nmax;
nmem=(n+1)*(n+1);
t=1;
f1=fopen("nlmax","w");/* in this file gets printed the maximum of the
calculated and actual solutions. for comparison */
//===============================
//Precalculem index2d 270400
// -1.64 -1.64 0.64 [66][66][65]
// int c2d[n+2][n+2][n+1];
// calci2d=malloc((n+2)*(n+2)*(n+1)*sizeof(int));
// calci2d = &c2d[0][0][0];
// if(calci2d==NULL)printf(" oopsies can't allocate calci2d %d \n",__LINE__);
// int i2d, j2d;
// i2d = j2d = 0;
// int* p = &c2d[0][0][0];
//
//printf(" %p ______ %p \n ", calci2d, p);
int i2d, j2d;
// printf("malloc calci2d\n");
calci2d = malloc((n+2)*sizeof(int**));
if(calci2d==NULL)printf(" oopsies can't allocate calci2d %d \n",__LINE__);
for(i2d=0; i2d < (n+2); i2d++){
// printf("malloc calci2d[%d]\n", i2d);
calci2d[i2d] = malloc((n+2)*sizeof(int*));
if(calci2d[i2d]==NULL)printf(" oopsies can't allocate calci2d[%d] %d \n", i2d,__LINE__);
for( j2d = 0; j2d < (n+2); j2d++){
/* code */
// printf("malloc calci2d[%d][%d]\n", i2d, j2d);
calci2d[i2d][j2d] = malloc((n+1)*sizeof(int));
if(calci2d[i2d][j2d]==NULL)printf(" oopsies can't allocate calci2d[%d][%d] %d \n", i2d,j2d, __LINE__);
}
}
int stride = 0;
i2d = j2d = 0;
int *p = &calci2d[0][0][0];
for(i2d=0; i2d < (n+2); i2d ++){
for(j2d=0; j2d < (n+2); j2d ++){
for(stride=0; stride < (n+1); stride ++){
calci2d[i2d][j2d][stride] = (i2d-1+n)%n + stride*((j2d-1+n)%n);
}
}
}
// while(i2d<n+2){
// // if(i<0) i+=stride;
// // else if(i>=stride) i-=stride;
// // if(j<0) j+=stride;
// // else if(j>=stride) j-=stride;
// // int aux = (i+stride*j);
// // return(i+stride*j);
//
// printf("computing calci2d[%d][%d][%d]", i2d, j2d, stride);
// *p = (i2d-1+n)%n + stride*((j2d-1+n)%n);
// printf(" = %d\n", (i2d-1+n)%n + stride*((j2d-1+n)%n));
// p++;
//
// if(j2d == 1){
// printf("%d == %d", calci2d[i2d][j2d][stride], (i2d-1+n)%n + stride*((j2d-1+n)%n));
// exit(0);
// }
//
// stride ++;
// if(stride == n+1){
// stride = 0;
// j2d++;
// if(j2d == n+2){
// j2d = 0;
// i2d++;
// }
// }
// } //while
//=============================
areal=malloc(nmem*sizeof(double));
if(areal==NULL)printf(" oopsies can't allocate areal %d \n",__LINE__);
ic(areal,n,t);//call initial condition if it's the first time through
freal=malloc(nmem*sizeof(double));
if(freal==NULL)printf(" oopsies can't allocate freal %d \n",__LINE__);
h=(xmax-xmin)/(double) n;
while(t < timemax){
funct(freal,areal,n,t);
mgrid(areal,n,freal,t);
//mgrid(areal,nmax,freal,t);
fprintf(f1,"%d %lf %lf \n",t,areal[index2d(n/4,n/4,n)],sin(t*tau));
fflush(f1);
t++;
}/* while */
printf( "imin: %d imax: %d\n", imin, imax );
printf( "jmin: %d jmax: %d\n", jmin, jmax );
printf( "stridemin: %d stridemax: %d\n", stridemin, stridemax );
fclose(f1);
free(areal);
free(freal);
}/* main */
AttrsPtr(const TCoords& p, Attributes *attrs) { pos = p; pa = &attrs[index2d(pos.x, pos.y)]; }