/* * WMM - Wave-matching method for mode analysis of dielectric waveguides * https://wmm.computational-photonics.eu/ */ /* * wmmmat.cpp * linear algebra routines on vectors, matrices represented as * *double, old FORTRAN format ... */ #include #include #include #define PI 3.14159265358979323846 #define PI_2 1.57079632679489661923 #define PI_4 0.78539816339744830962 #define Wu2 1.41421356237309504880 /* random numbers */ /* according to "Numerical Recipes in C, 'ran0'" */ #define RNidum_0 987654 long RNidum = RNidum_0; #define RNia 16807 #define RNim 2147483647 #define RNam (1.0/RNim) #define RNiq 127773 #define RNir 2836 double randomnum() { long k; double ans; k = RNidum/RNiq; RNidum = RNia*(RNidum-k*RNiq)-RNir*k; if(RNidum < 0) RNidum += RNim; ans = RNam*RNidum; return ans; } void initrand() { RNidum = RNidum_0; return; } /* set zero: m x n - matrix a */ void matto0(double *a, int m, int n) { int j; for(j=0; j<=m*n-1; ++j) { *a = 0.0; ++a; } return; } /* matrix-multiplication: c = a*b, m x n matrix a, n x l matrix b, -> m x l matrix c */ void matmatmult(double *a, int m, int n, double *b, int l, double *c) { int i, j, k, jn; double sum; for(i=0; i<=m-1; ++i) { for(j=0; j<=l-1; ++j) { sum = 0.0; jn = j*n; for(k=0; k<=n-1; ++k) sum += a[i+k*m]*b[k+jn]; c[i+j*m] = sum; } } return; } /* matrix-copy: b = a, m x n matrices a, b */ void matcopy(double *a, double *b, int m, int n) { int i; for(i=0; i<=m*n-1; ++i) { *b = *a; ++a; ++b; } return; } /* additive matrix-copy: b = b + a, m x n matrices a, b */ void addmat(double *a, double *b, int m, int n) { int i; for(i=0; i<=m*n-1; ++i) { *b += *a; ++a; ++b; } return; } /* matrix-comparison: a == b ? 1:ok, 0:no , m x n - matrices */ int matcompare(double *a, double *b, int m, int n) { int i, j; int eq; double md, d; eq = 1; md = 0.0; for(j=0; j<=n-1; ++j) { for(i=0; i<=m-1; ++i) { if(a[i+j*m] != b[i+j*m]) { d = fabs(a[i+j*m] - b[i+j*m]); fprintf(stderr, "[%d,%d]: %g # %g d: %g\n", i, j, a[i+j*m], b[i+j*m], d); eq = 0; if(d>md) md = d; } } } fprintf(stderr, "\n >>: %g\n", md); return eq; } /* transpose: b = a^t, m x n matrix a -> n x m matrix b*/ void transpose(double *a, int m, int n, double *b) { int i, j, in; for(i=0; i<=m-1; ++i) { in = i*n; for(j=0; j<=n-1; ++j) b[j+in] = a[i+j*m]; } return; } /* Cholesky-decomposition of a symmetrical, positive matrix */ /* 0: okay, 1: matrix is not positive numerically */ int choldc(double *mat, int dim, int testm) { int i, j, k; double *p; double sum; p = new double[dim]; for(i=0; i<=dim-1; ++i) { for(j=i; j<=dim-1; ++j) { sum = mat[i+j*dim]; for(k=i-1; k>=0; k--) sum -= mat[i+k*dim]*mat[j+k*dim]; if(i==j) { if(sum<=0.0) { delete[] p; return 1; } p[i] = sqrt(sum); } else mat[j+i*dim] = sum/p[i]; } } for(i=0; i<=dim-1; ++i) mat[i+i*dim] = p[i]; if(testm == 1) { for(i=0; i<=dim-2; ++i) { for(j=i+1; j<=dim-1; ++j) mat[i+j*dim] = 0.0; } } delete[] p; return 0; } /* backward substitution following Cholesky-decomposition */ /* calculate cmat as a solution of lmat*cmat=rmat, m x m - matrix lmat, left-triangular m x n - matrix cmat, m x n - matrix rmat */ void bwdsubst(double *lmat, int m, double *cmat, int n, double *rmat) { int i, k, l; int ld; double sum; for(l=0; l<=n-1; ++l) { ld = l*m; for(i=0; i<=m-1; ++i) { sum = rmat[i+ld]; for(k=i-1; k>=0; --k) sum -= lmat[i+k*m]*cmat[k+ld]; cmat[i+ld] = sum/lmat[i+i*m]; } } return; } /* backward substitution following Cholesky-decomposition */ /* calculate vector a as a solution of lmat^t * a = x, m x m - matrix lmat, left-triangular, m - dim vector a, m - dim vector x */ void vecbwdsubs(double *lmat, int m, double *a, double *x) { int i, k, im; double sum; for(i=m-1; i>=0; --i) { im = i*m; sum = x[i]; for(k=i+1; k<=m-1; ++k) sum -= lmat[k+im]*a[k]; a[i] = sum/lmat[i+im]; } return; } void savemat(double *mat, int m, int n, const char *nam); /* calculate a solution xvec of the linear system amat*xvec=bvec */ /* amat: symmetrical, real, positive n*n - matrix */ /* only the upper (right) part of amat must be supplied */ int solvesympos(double *a, int n, double *x, double *b) { int res; double *y; /* savemat(a, n, n, "amat"); savemat(b, n, 1, "bvec"); */ y = new double[n]; res = choldc(a, n, 0); if(res != 0) { fprintf(stderr,"\nSOLVESYMPOS: Matrix >| 0 !\n"); return 1; } bwdsubst(a, n, y, 1, b); vecbwdsubs(a, n, x, y); delete[] y; /* savemat(x, n, 1, "xvec"); */ return 0; } /* normalize vector */ double normalizevec(double *v, int n) { double *jp, s=0.0; int j; jp = v; for(j=0; j<=n-1; ++j) { s += (*jp)*(*jp); ++jp; } s = 1.0/sqrt(s); jp = v; for(j=0; j<=n-1; ++j) { *jp *= s; ++jp; } return 1.0/s; } /* scalar product of two vectors */ double scalarprod(double *a, double *b, int n) { double s=0.0; int j; for(j=0; j<=n-1; ++j) { s += (*a)*(*b); ++a; ++b; } return s; } /* save matrix to file, MATLAB format */ void savemat(double *mat, int m, int n, const char *nam) { FILE *dat; int a, b; double m1; fprintf(stderr, ">> %s\n", nam); dat = fopen(nam, "w+"); for(a=0; a<=m-1; ++a) { for(b=0; b<=n-1; ++b) { m1 = mat[a+b*m]; if(m1 != 0.0) fprintf(dat, "%d %d %.14g\n", a+1, b+1, m1); } } m1 = mat[m*n-1]; if(m1 == 0.0) fprintf(dat, "%d %d %.14g\n", m, n, 0.0); fclose(dat); return; } /* test of a n x n matrix for symmetry */ void testmatsym(double *mat, int n, double minel) { int a, b; double m1, m2; fprintf(stderr, "symmetry faults:\n"); for(a=0; a<=n-1; ++a) { for(b=0; b<=n-1; ++b) { m1 = mat[a+b*n]; m2 = mat[b+a*n]; if(fabs((m1-m2)/m1) > minel) fprintf(stderr, "[%d,%d] %.16g <-> [%d,%d] %.16g\n", a+1, b+1, m1, b+1, a+1, m2); } } return; } /* copy dm x dn block at row l, column c from m x n matrix a to b */ void extractbloc(double *a, int m, int n, int l, int c, double *b, int dm, int dn) { int j, k, kdm, kcml; j = n; for(k=0; k<=dn-1; ++k) { kdm = k*dm; kcml = l+(k+c)*m; for(j=0; j<=dm-1; ++j) b[j+kdm] = a[j+kcml]; } return; } /* copy dm x dn matrix b at row l, column c to m x n matrix a */ void insertbloc(double *b, int dm, int dn, double *a, int m, int n, int l, int c) { int j, k, kdm, kcml; j = n; for(k=0; k<=dn-1; ++k) { kdm = k*dm; kcml = l+(k+c)*m; for(j=0; j<=dm-1; ++j) a[j+kcml] = b[j+kdm]; } return; } /* enforce symmetry */ void restoresymmetry(double *mat, int dim) { int a, b; double el1, el2, mel; for(a=0; a<=dim-2; ++a) { for(b=a+1; b<=dim-1; ++b) { el1 = mat[a+b*dim]; el2 = mat[b+a*dim]; mel = 0.5*(el1+el2); mat[a+b*dim] = mel; mat[b+a*dim] = mel; } } return; } /* ------------------------------------------------------------------------ */ /* "band matrices": first and last entry in each row are registered */ typedef struct { int dim; /* Dimension */ int *fe; /* first entries */ int *le; /* last entries */ double *mat; } bandmat; /* matrix m -> band format */ bandmat initbandmat(double *m, int d) { int j, f, l; bandmat bm; bm.dim = d; bm.mat = m; bm.fe = new int[d]; bm.le = new int[d]; for(j=0; j<=d-1; ++j) { f=0; while(m[j+f*d]==0.0 && fj) --l; bm.fe[j] = f; bm.le[j] = l; } return bm; } /* remove band format */ double *stripbandmat(bandmat bm) { delete[] bm.fe; delete[] bm.le; return bm.mat; } /* Cholesky-decomposition of a symmetrical, positive matrix */ /* band format */ /* 0: okay, 1: matrix is not positive numerically */ int bmcholdc(bandmat bm, int testm) { int i, j, k, d; double *p; double sum, e; double *m; int *f, *l; d = bm.dim; m = bm.mat; f = bm.fe; l = bm.le; p = new double[d]; for(i=0; i<=d-1; ++i) { for(j=i; j<=d-1; ++j) { sum = m[i+j*d]; for(k=i-1; k>=f[i] && k>=f[j]; k--) sum -= m[i+k*d]*m[j+k*d]; if(i==j) { if(sum<=0.0) { delete[] p; return 1; } p[i] = sqrt(sum); } else { e = sum/p[i]; m[j+i*d] = e; if(i=f[i]; --k) sum -= lmat[i+k*m]*cmat[k+ld]; cmat[i+ld] = sum/lmat[i+i*m]; } } return; } /* backward substitution following Cholesky-decomposition */ /* calculate vector a as a solution of lm^t * a = x, m x m - matrix lmat, left-triangular, band format, m - dim vector a, m - dim vector x */ void bmvecbwdsubs(bandmat lm, int m, double *a, double *x) { int i, k, im; double sum; double *lmat; lmat = lm.mat; for(i=m-1; i>=0; --i) { im = i*m; sum = x[i]; for(k=i+1; k<=m-1; ++k) sum -= lmat[k+im]*a[k]; a[i] = sum/lmat[i+im]; } return; } /* ------------------------------------------------------------------------ */ /* small eigenvalue / corresp. eigenvector of a real symmetrc matrix */ /* only the upper (right) part has to be supplied */ /* dim: dimension of matrix smat */ /* mode 0: calculate eigenvalue only 1: calculate eigenvalue and eigenvector ev */ /* evidx: index of the required eigenvalue, 0: the lowest, 1: the next larger */ /* based on: "tred2.c", "tqli.c", "dpythag.c" (C) Copr. 1986-92 Numerical Recipes Software 5.)13. */ double dpythag(double a, double b) { double absa, absb; double absaq, absbq; absa = fabs(a); absb = fabs(b); absaq = absa*absa; absbq = absb*absb; if (absa > absb) return absa*sqrt(1.0+absbq/absaq); else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+absaq/absbq)); } double smallesteigvalnumrec(double *smat, int dim, int mode, double *ev, int evidx) { int l, k, j, i; double scale, hh, h, g, f; int m, iter; double s, r, p, dd, c, b; double *e, *d; int idim, kdim, ldim, jdim; e = new double[dim]; d = new double[dim]; for(i=1; i<=dim-1; ++i) { idim = i*dim; for(j=0; j<=i-1; ++j) smat[i+j*dim] = smat[j+idim]; } for (i=dim-1;i>=1;i--) { l=i-1; ldim = l*dim; h=scale=0.0; if (l > 0) { for (k=0;k<=l;k++) scale += fabs(smat[i+k*dim]); if (scale == 0.0) e[i]=smat[i+ldim]; else { for (k=0;k<=l;k++) { kdim = k*dim; smat[i+kdim] /= scale; h += smat[i+kdim]*smat[i+kdim]; } f=smat[i+ldim]; g=(f >= 0.0 ? -sqrt(h) : sqrt(h)); e[i]=scale*g; h -= f*g; smat[i+ldim]=f-g; f=0.0; for (j=0;j<=l;j++) { jdim = j*dim; smat[j+i*dim]=smat[i+jdim]/h; g=0.0; for (k=0;k<=j;k++) g += smat[j+k*dim] *smat[i+k*dim]; for (k=j+1;k<=l;k++) g += smat[k+jdim] *smat[i+k*dim]; e[j]=g/h; f += e[j]*smat[i+jdim]; } hh=f/(h+h); for (j=0;j<=l;j++) { f=smat[i+j*dim]; e[j]=g=e[j]-hh*f; for (k=0;k<=j;k++) smat[j+k*dim] -= (f*e[k]+g*smat[i+k*dim]); } } } else e[i]=smat[i+ldim]; d[i]=h; } d[0]=0.0; e[0]=0.0; if(mode == 0) { for (i=0;i<=dim-1;i++) d[i]=smat[i+i*dim]; } else { for (i=0;i<=dim-1;i++) { idim = i*dim; l=i-1; if (d[i]) { for (j=0;j<=l;j++) { jdim = j*dim; g=0.0; for (k=0;k<=l;k++) g += smat[i+k*dim]*smat[k+jdim]; for (k=0;k<=l;k++) smat[k+jdim] -= g*smat[k+idim]; } } d[i]=smat[i+idim]; smat[i+idim]=1.0; for (j=0;j<=l;j++) smat[j+idim]=smat[i+j*dim]=0.0; } } /* ^: Red. auf Tridiagonalform, >: Diagon. der Tridiagonalmatrix */ for (i=1;i<=dim-1;i++) e[i-1]=e[i]; e[dim-1]=0.0; for (l=0;l<=dim-1;l++) { iter=0; do { for (m=l+1;m<=dim-1;m++) { dd=fabs(d[m-1])+fabs(d[m]); if ((double)(fabs(e[m-1])+dd) == dd) break; } if (m != l+1) { if (iter++ == 30) { fprintf(stderr,"\nSMEV: NR: Too many iterations in tqli !\n "); exit(1); } g=(d[l+1]-d[l])/(2.0*e[l]); r=dpythag(g,1.0); g=d[m-1]-d[l] +e[l]/(g + (g>=0.0 ? fabs(r) : -fabs(r))); s=c=1.0; p=0.0; for (i=m-1;i>=l+1;i--) { f=s*e[i-1]; b=c*e[i-1]; e[i]=(r=dpythag(f,g)); if (r == 0.0) { d[i] -= p; e[m-1]=0.0; break; } s=f/r; c=g/r; g=d[i]-p; r=(d[i-1]-g)*s+2.0*c*b; d[i]=g+(p=s*r); g=c*r-b; if(mode == 1) { for (k=0;k<=dim-1;k++) { f=smat[k+i*dim]; smat[k+i*dim] =s*smat[k+(i-1)*dim]+c*f; smat[k+(i-1)*dim] =c*smat[k+(i-1)*dim]-s*f; } } } if (r == 0.0 && i >= l+1) continue; d[l] -= p; e[l]=g; e[m-1]=0.0; } } while (m != l+1); } i = 0; for(j=1; j<=dim-1; ++j) { if(d[j]| 0 !\n"); *w = -1.0; stripbandmat(bmfa); delete[] y; delete[] b; delete[] ob; return 1; } numit = 0; while(fabs((e-eo)/eo) >= EVTol) { tp = b; b = ob; ob = tp; bmbwdsubst(bmfa, dim, y, 1, ob); bmvecbwdsubs(bmfa, dim, b, y); s = normalizevec(b, dim); eo = e; e = scalarprod(b, ob, dim)/s; ++numit; if(numit > 1000) { fprintf(stderr,"\nSMEV: #Iterations > 1000 !\n"); *w = e; if(mode == 1) matcopy(b, ev, dim, 1); stripbandmat(bmfa); delete[] y; delete[] b; delete[] ob; return 0; } } *w = e; if(mode == 1) matcopy(b, ev, dim, 1); stripbandmat(bmfa); delete[] y; delete[] b; delete[] ob; return 0; } /* ------------------------------------------------------------------------ */ double smallesteigval(double *snmat, int dim, int mode, double *ev, int evidx, int pos) { double w; int p; if(evidx == 0 && pos == 1) { p = smallesteigvalinvit(snmat, dim, mode, ev, &w); if(p != 0) { fprintf(stderr, "\nSMEV: inverse iteration failed !\n"); exit(1); } return w; } else { return smallesteigvalnumrec(snmat, dim, mode, ev, evidx); } return 0.0; } /* ------------------------------------------------------------------------ */ /* all eigenvalues / corresp. eigenvectors of a real symmetrc matrix */ /* only the upper (right) part has to be supplied */ /* dim: dimension of matrix smat */ /* mode 0: calculate eigenvalue only 1: calculate eigenvalue and eigenvector ev */ /* based on: "tred2.c", "tqli.c", "dpythag.c" (C) Copr. 1986-92 Numerical Recipes Software 5.)13. */ void eigvalnumrec(double *smat, int dim, int mode, double *eval) { int l, k, j, i; double scale, hh, h, g, f; int m, iter; double s, r, p, dd, c, b; double *e, *d; int idim, kdim, ldim, jdim; e = new double[dim]; d = new double[dim]; for(i=1; i<=dim-1; ++i) { idim = i*dim; for(j=0; j<=i-1; ++j) smat[i+j*dim] = smat[j+idim]; } for (i=dim-1;i>=1;i--) { l=i-1; ldim = l*dim; h=scale=0.0; if (l > 0) { for (k=0;k<=l;k++) scale += fabs(smat[i+k*dim]); if (scale == 0.0) e[i]=smat[i+ldim]; else { for (k=0;k<=l;k++) { kdim = k*dim; smat[i+kdim] /= scale; h += smat[i+kdim]*smat[i+kdim]; } f=smat[i+ldim]; g=(f >= 0.0 ? -sqrt(h) : sqrt(h)); e[i]=scale*g; h -= f*g; smat[i+ldim]=f-g; f=0.0; for (j=0;j<=l;j++) { jdim = j*dim; smat[j+i*dim]=smat[i+jdim]/h; g=0.0; for (k=0;k<=j;k++) g += smat[j+k*dim] *smat[i+k*dim]; for (k=j+1;k<=l;k++) g += smat[k+jdim] *smat[i+k*dim]; e[j]=g/h; f += e[j]*smat[i+jdim]; } hh=f/(h+h); for (j=0;j<=l;j++) { f=smat[i+j*dim]; e[j]=g=e[j]-hh*f; for (k=0;k<=j;k++) smat[j+k*dim] -= (f*e[k]+g*smat[i+k*dim]); } } } else e[i]=smat[i+ldim]; d[i]=h; } d[0]=0.0; e[0]=0.0; if(mode == 0) { for (i=0;i<=dim-1;i++) d[i]=smat[i+i*dim]; } else { for (i=0;i<=dim-1;i++) { idim = i*dim; l=i-1; if (d[i]) { for (j=0;j<=l;j++) { jdim = j*dim; g=0.0; for (k=0;k<=l;k++) g += smat[i+k*dim]*smat[k+jdim]; for (k=0;k<=l;k++) smat[k+jdim] -= g*smat[k+idim]; } } d[i]=smat[i+idim]; smat[i+idim]=1.0; for (j=0;j<=l;j++) smat[j+idim]=smat[i+j*dim]=0.0; } } /* ^: Red. auf Tridiagonalform, >: Diagon. der Tridiagonalmatrix */ for (i=1;i<=dim-1;i++) e[i-1]=e[i]; e[dim-1]=0.0; for (l=0;l<=dim-1;l++) { iter=0; do { for (m=l+1;m<=dim-1;m++) { dd=fabs(d[m-1])+fabs(d[m]); if ((double)(fabs(e[m-1])+dd) == dd) break; } if (m != l+1) { if (iter++ == 30) { fprintf(stderr,"\nSMEV: NR: Too many iterations in tqli !\n "); exit(1); } g=(d[l+1]-d[l])/(2.0*e[l]); r=dpythag(g,1.0); g=d[m-1]-d[l] +e[l]/(g + (g>=0.0 ? fabs(r) : -fabs(r))); s=c=1.0; p=0.0; for (i=m-1;i>=l+1;i--) { f=s*e[i-1]; b=c*e[i-1]; e[i]=(r=dpythag(f,g)); if (r == 0.0) { d[i] -= p; e[m-1]=0.0; break; } s=f/r; c=g/r; g=d[i]-p; r=(d[i-1]-g)*s+2.0*c*b; d[i]=g+(p=s*r); g=c*r-b; if(mode == 1) { for (k=0;k<=dim-1;k++) { f=smat[k+i*dim]; smat[k+i*dim] =s*smat[k+(i-1)*dim]+c*f; smat[k+(i-1)*dim] =c*smat[k+(i-1)*dim]-s*f; } } } if (r == 0.0 && i >= l+1) continue; d[l] -= p; e[l]=g; e[m-1]=0.0; } } while (m != l+1); } for(i=0; i<=dim-1; ++i) eval[i] = d[i]; delete[] e; delete[] d; return; } /* ------------------------------------------------------------------------ */