/* * METRIC --- Mode expansion modeling in integrated optics / photonics * http://metric.computational-photonics.eu/ */ /* * matrix.cpp * Basic vectors, matrices, third order arrays, some linear algebra routines */ #include #include #include #include #include #include"inout.h" #include"complex.h" #include"matrix.h" #ifdef INC_EIGEN #include"Eigen/Dense" #include"Eigen/LU" #include"Eigen/Eigenvalues" #endif // #define INITARR // remove/place comment to enable/disable // initialization to zero in all array constructors /* error message */ void matrixerror(const char *m) { fprintf(stderr, "\nmatrix: %s.\n", m); exit(1); } /* ------------------------------------------------------------------- */ #ifdef INC_EIGEN // prepare use of numerical routines from the Eigen packages #define complex std::complex #define dynamic Eigen::Dynamic typedef Eigen::Matrix E_Dmatrix; typedef Eigen::Matrix E_Dvector; typedef Eigen::Matrix E_Cmatrix; typedef Eigen::Matrix E_Cvector; /* conversion between complex types */ inline Complex convCv(const complex& z) { return Complex(z.real(), z.imag()); } inline complex convCv(const Complex& z) { return complex(z.re, z.im); } /* conversion from / to Eigen types */ E_Dvector convEo(const Dvector& a) { E_Dvector b(a.nel); for(int i=0; i<=a.nel-1; ++i) b(i) = a(i); return b; } E_Dmatrix convEo(const Dmatrix& a) { E_Dmatrix b(a.r, a.c); for(int i=0; i<=a.r-1; ++i) { for(int j=0; j<=a.c-1; ++j) b(i, j) = a(i, j); } return b; } E_Cvector convEo(const Cvector& a) { E_Cvector b(a.nel); for(int i=0; i<=a.nel-1; ++i) b(i) = convCv(a(i)); return b; } E_Cmatrix convEo(const Cmatrix& a) { E_Cmatrix b(a.r, a.c); for(int i=0; i<=a.r-1; ++i) { for(int j=0; j<=a.c-1; ++j) b(i, j) = convCv(a(i, j)); } return b; } Dvector convEo(const E_Dvector& a) { Dvector b(a.rows()); for(int i=0; i<=a.rows()-1; ++i) b(i) = a(i); return b; } Dmatrix convEo(const E_Dmatrix& a) { Dmatrix b(a.rows(), a.cols()); for(int i=0; i<=a.rows()-1; ++i) { for(int j=0; j<=a.cols()-1; ++j) b(i, j) = a(i, j); } return b; } Cvector convEo(const E_Cvector& a) { Cvector b(a.rows()); for(int i=0; i<=a.rows()-1; ++i) b(i) = convCv(a(i)); return b; } Cmatrix convEo(const E_Cmatrix& a) { Cmatrix b(a.rows(), a.cols()); for(int i=0; i<=a.rows()-1; ++i) { for(int j=0; j<=a.cols()-1; ++j) b(i, j) = convCv(a(i, j)); } return b; } #endif /* ------------------------------------------------------------------- */ /* Cvector: complex vector */ /* initialize */ Cvector::Cvector() : nel(0) { m = NULL; } Cvector::Cvector(int ne) : nel(ne) { m = new Complex[ne]; #ifdef INITARR init(CC0); #endif } Cvector::Cvector(const Dvector& rp) : nel(rp.nel) { m = new Complex[rp.nel]; for(int x=0; x<=nel-1; ++x) m[x] = Complex(rp(x), 0.0); } Cvector::Cvector(const Dvector& rp, const Dvector& ip) : nel(rp.nel) { if(rp.nel != ip.nel) matrixerror("Cvector(Dvector, Dvector): dimensions"); m = new Complex[rp.nel]; for(int x=0; x<=nel-1; ++x) m[x] = Complex(rp(x), ip(x)); } /* destroy */ Cvector::~Cvector() { if(m != NULL) delete[] m; m = NULL; nel = 0; } /* copy constructor */ Cvector::Cvector(const Cvector& s) : nel(s.nel) { m = new Complex[nel]; Complex* sp = s.m; Complex* mp = m; for(int i=0; i<=nel-1; ++i) *mp++ = *sp++; } /* assignment */ Cvector& Cvector::operator=(const Cvector& s) { if(this != &s) { if(m != NULL) delete[] m; nel = s.nel; m = new Complex[nel]; Complex* sp = s.m; Complex* mp = m; for(int i=0; i<=nel-1; ++i) *mp++ = *sp++; } return *this; } /* free allocated memory */ void Cvector::strip() { nel = 0; if(m != NULL) delete[] m; m = NULL; } /* set all elements to d */ void Cvector::init(Complex c) { Complex* mp = m; for(int i=0; i<=nel-1; ++i) *mp++ = c; return; } /* complex conjugate */ void Cvector::conj() { Complex* mp = m; for(int i=0; i<=nel-1; ++i) {*mp = (*mp).conj(); mp++;} return; } /* output to FILE dat */ void Cvector::write(FILE *dat) const { if(dat == stderr || dat == stdout) { fprintf(dat, "Elements: %d\n", nel); for(int i=0; i<=nel-1; ++i) m[i].write(dat); } else { comment("nel", dat); fputint(nel, dat); comment("[]", dat); for(int i=0; i<=nel-1; ++i) m[i].write(dat); } return; } /* output to MATLAB script file 'name.m' */ void Cvector::mfile(const char *name) const { FILE *f; int l, i; char n[20]; n[0] = 'v'; n[1] = '_'; l=2; while(l<=15 && name[l-2] != 0) {n[l] = name[l-2]; ++l;} n[l] = '.'; ++l; n[l] = 'm'; ++l; n[l] = 0; fprintf(stderr, "(%d) >> %s\n", nel, n); f = fopen(n, "w+"); fprintf(f, "%s = [\n", name); for(i=0; i<=nel-1; ++i) fprintf(f, "%.15g+i*%.15g\n", m[i].re, m[i].im); fprintf(f, "];\n"); fclose(f); return; } /* input from FILE dat */ void Cvector::read(FILE *dat) { if(dat == stderr || dat == stdout) { matrixerror("Cvector: read"); } else { if(m != NULL) delete[] m; nel = fgetint(dat); m = new Complex[nel]; for(int i=0; i<=nel-1; ++i) m[i].read(dat); } return; } /* real part */ Dvector Cvector::re() const { Dvector nv(nel); double *np = nv.m; Complex *mp = m; for(int x=0; x<=nel-1; ++x) *np++ = (*mp++).re; return nv; } /* imaginary part */ Dvector Cvector::im() const { Dvector nv(nel); double *np = nv.m; Complex *mp = m; for(int x=0; x<=nel-1; ++x) *np++ = (*mp++).im; return nv; } /* absolute values of the elements */ Dvector Cvector::abs() const { Dvector nv(nel); double *np = nv.m; Complex *mp = m; for(int x=0; x<=nel-1; ++x) *np++ = (*mp++).abs(); return nv; } /* squared absolute values of the elements */ Dvector Cvector::sqabs() const { Dvector nv(nel); double *np = nv.m; Complex *mp = m; for(int x=0; x<=nel-1; ++x) *np++ = (*mp++).sqabs(); return nv; } /* the arguments (phase) of the original entries */ Dvector Cvector::arg() const { Dvector nv(nel); double *np = nv.m; Complex *mp = m; for(int x=0; x<=nel-1; ++x) *np++ = (*mp++).arg(); return nv; } /* extract a subvector: n entries starting at position x */ Cvector Cvector::subvector(int n, int x) const { if(x < 0 || x >= nel) matrixerror("Cvector: subvector: dimensions, (1)"); if(n < 0) matrixerror("Cvector: subvector: dimensions, (2)"); if(x+n > nel) matrixerror("Cvector: subvector: dimensions, (3)"); Cvector s(n); Complex* sp = s.m; Complex* mp = m; mp = &(m[x]); for(int i=0; i<=n-1; ++i) *sp++ = *mp++; return s; } /* set the entries beginning at position x, y to the values of the supplied (smaller) matrix */ void Cvector::setsubvector(const Cvector &s, int x) { if(x < 0 || x >= nel) matrixerror("Cvector: setsubvector: dimensions, (1)"); if(x+s.nel > nel) matrixerror("Cvector: setsubvector: dimensions, (2)"); Complex* sp = s.m; Complex* mp = m; mp = &(m[x]); for(int i=0; i<=s.nel-1; ++i) *mp++ = *sp++; return; } /* append an entry to the end of the vector */ void Cvector::append(Complex e) { Complex *n; n = new Complex[nel+1]; Complex* np = n; Complex* mp = m; for(int i=0; i<=nel-1; ++i) *np++ = *mp++; *np = e; if(m != NULL) delete[] m; m = n; ++nel; return; } /* insert value e at position p, shift the following elements accordingly */ void Cvector::insert(Complex e, int p) { Complex *n; n = new Complex[nel+1]; Complex* np = n; Complex* mp = m; for(int i=0; i<=p-1; ++i) *np++ = *mp++; *np++ = e; for(int i=p; i<=nel-1; ++i) *np++ = *mp++; if(m != NULL) delete[] m; m = n; ++nel; return; } /* remove the element at position p, shift the following elements accordingly */ void Cvector::remove(int p) { Complex *n; n = new Complex[nel-1]; Complex* np = n; Complex* mp = m; for(int i=0; i<=p-1; ++i) *np++ = *mp++; ++mp; for(int i=p+1; i<=nel-1; ++i) *np++ = *mp++; if(m != NULL) delete[] m; m = n; --nel; return; } /* the Euclidean norm, squared */ double Cvector::sqnorm() const { double s = 0.0; for(int i=0; i<=nel-1; ++i) s += m[i].sqabs(); return s; } /* the Euclidean norm */ double Cvector::norm() const { return sqrt(sqnorm()); } /* maximum absolute value of the vector elements, corresponding index */ double Cvector::max() const { double s = m[0].sqabs(); for(int i=1; i<=nel-1; ++i) { if(m[i].sqabs() > s) s = m[i].sqabs(); } return sqrt(s); } double Cvector::max(int& mi) const { double s = m[0].sqabs(); mi = 0; for(int i=1; i<=nel-1; ++i) { if(m[i].sqabs() > s) { s = m[i].sqabs(); mi = i; } } return sqrt(s); } /* ------------------------------------------------------------------- */ /* Dvector: double vector */ /* initialize */ Dvector::Dvector() : nel(0) { m = NULL; } Dvector::Dvector(int ne) : nel(ne) { m = new double[ne]; #ifdef INITARR init(0.0); #endif } /* destroy */ Dvector::~Dvector() { if(m != NULL) delete[] m; m = NULL; nel = 0; } /* copy constructor */ Dvector::Dvector(const Dvector& s) : nel(s.nel) { m = new double[nel]; double* sp = s.m; double* mp = m; for(int i=0; i<=nel-1; ++i) *mp++ = *sp++; } /* assignment */ Dvector& Dvector::operator=(const Dvector& s) { if(this != &s) { if(m != NULL) delete[] m; nel = s.nel; m = new double[nel]; double* sp = s.m; double* mp = m; for(int i=0; i<=nel-1; ++i) *mp++ = *sp++; } return *this; } /* free allocated memory */ void Dvector::strip() { nel = 0; if(m != NULL) delete[] m; m = NULL; } /* set all elements to d */ void Dvector::init(double d) { double* mp = m; for(int i=0; i<=nel-1; ++i) *mp++ = d; return; } /* output to FILE dat */ void Dvector::write(FILE *dat) const { if(dat == stderr || dat == stdout) { fprintf(dat, "Elements: %d\n", nel); if(nel <= 5) for(int i=0; i<=nel-1; ++i) fprintf(dat, "%g ", m[i]); else for(int i=0; i<=nel-1; ++i) fprintf(dat, "%g\n", m[i]); fprintf(dat, "\n"); } else { comment("nel", dat); fputint(nel, dat); comment("[]", dat); for(int i=0; i<=nel-1; ++i) fputdouble(m[i], dat); } return; } /* output to MATLAB script file 'name.m' */ void Dvector::mfile(const char *name) const { FILE *f; int l, i; char n[20]; n[0] = 'v'; n[1] = '_'; l=2; while(l<=15 && name[l-2] != 0) {n[l] = name[l-2]; ++l;} n[l] = '.'; ++l; n[l] = 'm'; ++l; n[l] = 0; fprintf(stderr, "(%d) >> %s\n", nel, n); f = fopen(n, "w+"); fprintf(f, "%s = [\n", name); for(i=0; i<=nel-1; ++i) fprintf(f, "%.15g\n", m[i]); fprintf(f, "];\n"); fclose(f); return; } /* input from FILE dat */ void Dvector::read(FILE *dat) { if(dat == stderr || dat == stdout) { matrixerror("Dvector: read"); } else { if(m != NULL) delete[] m; nel = fgetint(dat); m = new double[nel]; for(int i=0; i<=nel-1; ++i) m[i] = fgetdouble(dat); } return; } /* extract a subvector: n entries starting at position x */ Dvector Dvector::subvector(int n, int x) const { if(x < 0 || x >= nel) matrixerror("Dvector: subvector: dimensions, (1)"); if(n < 0) matrixerror("Dvector: subvector: dimensions, (2)"); if(x+n > nel) matrixerror("Dvector: subvector: dimensions, (3)"); Dvector s(n); double* sp = s.m; double* mp = m; mp = &(m[x]); for(int i=0; i<=n-1; ++i) *sp++ = *mp++; return s; } /* set the entries beginning at position x, y to the values of the supplied (smaller) matrix */ void Dvector::setsubvector(const Dvector &s, int x) { if(x < 0 || x >= nel) matrixerror("Dvector: setsubvector: dimensions, (1)"); if(x+s.nel > nel) matrixerror("Dvector: setsubvector: dimensions, (2)"); double* sp = s.m; double* mp = m; mp = &(m[x]); for(int i=0; i<=s.nel-1; ++i) *mp++ = *sp++; return; } /* append an entry to the end of the vector */ void Dvector::append(double e) { double *n; n = new double[nel+1]; double* np = n; double* mp = m; for(int i=0; i<=nel-1; ++i) *np++ = *mp++; *np = e; if(m != NULL) delete[] m; m = n; ++nel; return; } /* insert value e at position p, shift the following elements accordingly */ void Dvector::insert(double e, int p) { double *n; n = new double[nel+1]; double* np = n; double* mp = m; for(int i=0; i<=p-1; ++i) *np++ = *mp++; *np++ = e; for(int i=p; i<=nel-1; ++i) *np++ = *mp++; if(m != NULL) delete[] m; m = n; ++nel; return; } /* remove the element at position p, shift the following elements accordingly */ void Dvector::remove(int p) { double *n; n = new double[nel-1]; double* np = n; double* mp = m; for(int i=0; i<=p-1; ++i) *np++ = *mp++; ++mp; for(int i=p+1; i<=nel-1; ++i) *np++ = *mp++; if(m != NULL) delete[] m; m = n; --nel; return; } /* the Euclidean norm, squared */ double Dvector::sqnorm() const { double s = 0.0; for(int i=0; i<=nel-1; ++i) s += m[i]*m[i]; return s; } /* the Euclidean norm */ double Dvector::norm() const { return sqrt(sqnorm()); } /* sort the vector elements: d>=0: ascending, else descending */ void Dvector::sort(int d) { if(nel <= 1) return; int j, k, maxi; double maxel, t; if(d>=0) { for(j=0; j<=nel-2; ++j) { maxi = j; maxel = m[j]; for(k=j+1; k<=nel-1; ++k) { if(maxel>m[k]) { maxel = m[k]; maxi = k; } } if(j != maxi) { t = m[j]; m[j] = m[maxi]; m[maxi] = t; } } return; } for(j=0; j<=nel-2; ++j) { maxi = j; maxel = m[j]; for(k=j+1; k<=nel-1; ++k) { if(maxel s) s = m[i]; } return s; } double Dvector::max(int& mi) const { double s = m[0]; mi = 0; for(int i=1; i<=nel-1; ++i) { if(m[i] > s) { s = m[i]; mi = i; } } return s; } /* ------------------------------------------------------------------- */ /* Ivector: int vector */ /* initialize */ Ivector::Ivector() : nel(0) { m = NULL; } Ivector::Ivector(int ne) : nel(ne) { m = new int[ne]; #ifdef INITARR init(0); #endif } /* destroy */ Ivector::~Ivector() { if(m != NULL) delete[] m; m = NULL; nel = 0; } /* copy constructor */ Ivector::Ivector(const Ivector& s) : nel(s.nel) { m = new int[nel]; int* sp = s.m; int* mp = m; for(int i=0; i<=nel-1; ++i) *mp++ = *sp++; } /* assignment */ Ivector& Ivector::operator=(const Ivector& s) { if(this != &s) { if(m != NULL) delete[] m; nel = s.nel; m = new int[nel]; int* sp = s.m; int* mp = m; for(int i=0; i<=nel-1; ++i) *mp++ = *sp++; } return *this; } /* free allocated memory */ void Ivector::strip() { nel = 0; if(m != NULL) delete[] m; m = NULL; } /* set all elements to d */ void Ivector::init(int d) { int* mp = m; for(int i=0; i<=nel-1; ++i) *mp++ = d; return; } /* output to FILE dat */ void Ivector::write(FILE *dat) const { if(dat == stderr || dat == stdout) { fprintf(dat, "Elements: %d\n", nel); for(int i=0; i<=nel-1; ++i) fprintf(dat, "%d ", m[i]); fprintf(dat, "\n"); } else { comment("nel", dat); fputint(nel, dat); comment("[]", dat); for(int i=0; i<=nel-1; ++i) fputint(m[i], dat); } return; } /* output to MATLAB script file 'name.m' */ void Ivector::mfile(const char *name) const { FILE *f; int l, i; char n[20]; n[0] = 'v'; n[1] = '_'; l=2; while(l<=15 && name[l-2] != 0) {n[l] = name[l-2]; ++l;} n[l] = '.'; ++l; n[l] = 'm'; ++l; n[l] = 0; fprintf(stderr, "(%d) >> %s\n", nel, n); f = fopen(n, "w+"); fprintf(f, "%s = [\n", name); for(i=0; i<=nel-1; ++i) fprintf(f, "%d\n", m[i]); fprintf(f, "];\n"); fclose(f); return; } /* input from FILE dat */ void Ivector::read(FILE *dat) { if(dat == stderr || dat == stdout) { matrixerror("Ivector: read"); } else { if(m != NULL) delete[] m; nel = fgetint(dat); m = new int[nel]; for(int i=0; i<=nel-1; ++i) m[i] = fgetint(dat); } return; } /* extract a subvector: n entries starting at position x */ Ivector Ivector::subvector(int n, int x) const { if(x < 0 || x >= nel) matrixerror("Ivector: subvector: dimensions, (1)"); if(n < 0) matrixerror("Ivector: subvector: dimensions, (2)"); if(x+n > nel) matrixerror("Ivector: subvector: dimensions, (3)"); Ivector s(n); for(int i=0; i<=n-1; ++i) s(i) = (*this)(x+i); return s; } /* set the entries beginning at position x, y to the values of the supplied (smaller) matrix */ void Ivector::setsubvector(const Ivector &s, int x) { if(x < 0 || x >= nel) matrixerror("Ivector: setsubvector: dimensions, (1)"); if(x+s.nel > nel) matrixerror("Ivector: setsubvector: dimensions, (2)"); for(int i=0; i<=s.nel-1; ++i) (*this)(x+i) = s(i); return; } /* append an entry to the end of the vector */ void Ivector::append(int e) { int *n; n = new int[nel+1]; int* np = n; int* mp = m; for(int i=0; i<=nel-1; ++i) *np++ = *mp++; *np = e; if(m != NULL) delete[] m; m = n; ++nel; return; } /* insert value e at position p, shift the following elements accordingly */ void Ivector::insert(int e, int p) { int *n; n = new int[nel+1]; int* np = n; int* mp = m; for(int i=0; i<=p-1; ++i) *np++ = *mp++; *np++ = e; for(int i=p; i<=nel-1; ++i) *np++ = *mp++; if(m != NULL) delete[] m; m = n; ++nel; return; } /* remove the element at position p, shift the following elements accordingly */ void Ivector::remove(int p) { int *n; n = new int[nel-1]; int* np = n; int* mp = m; for(int i=0; i<=p-1; ++i) *np++ = *mp++; ++mp; for(int i=p+1; i<=nel-1; ++i) *np++ = *mp++; if(m != NULL) delete[] m; m = n; --nel; return; } /* the Euclidean norm, squared */ double Ivector::sqnorm() const { double s = 0.0; for(int i=0; i<=nel-1; ++i) s += m[i]*m[i]; return s; } /* the Euclidean norm */ double Ivector::norm() const { return sqrt(sqnorm()); } /* sort the vector elements: d>=0: ascending, else descending */ void Ivector::sort(int d) { if(nel <= 1) return; int j, k, maxi; int maxel, t; if(d>=0) { for(j=0; j<=nel-2; ++j) { maxi = j; maxel = m[j]; for(k=j+1; k<=nel-1; ++k) { if(maxel>m[k]) { maxel = m[k]; maxi = k; } } if(j != maxi) { t = m[j]; m[j] = m[maxi]; m[maxi] = t; } } return; } for(j=0; j<=nel-2; ++j) { maxi = j; maxel = m[j]; for(k=j+1; k<=nel-1; ++k) { if(maxel s) s = m[i]; } return s; } int Ivector::max(int& mi) const { int s = m[0]; mi = 0; for(int i=1; i<=nel-1; ++i) { if(m[i] > s) { s = m[i]; mi = i; } } return s; } /* ------------------------------------------------------------------- */ /* Bvector: bool vector */ /* initialize */ Bvector::Bvector() : nel(0) { m = NULL; } Bvector::Bvector(int ne) : nel(ne) { m = new bool[ne]; #ifdef INITARR init(false); #endif } /* destroy */ Bvector::~Bvector() { if(m != NULL) delete[] m; m = NULL; nel = 0; } /* copy constructor */ Bvector::Bvector(const Bvector& s) : nel(s.nel) { m = new bool[nel]; bool* sp = s.m; bool* mp = m; for(int i=0; i<=nel-1; ++i) *mp++ = *sp++; } /* assignment */ Bvector& Bvector::operator=(const Bvector& s) { if(this != &s) { if(m != NULL) delete[] m; nel = s.nel; m = new bool[nel]; bool* sp = s.m; bool* mp = m; for(int i=0; i<=nel-1; ++i) *mp++ = *sp++; } return *this; } /* free allocated memory */ void Bvector::strip() { nel = 0; if(m != NULL) delete[] m; m = NULL; } /* set all elements to d */ void Bvector::init(bool b) { bool* mp = m; for(int i=0; i<=nel-1; ++i) *mp++ = b; return; } /* output to FILE dat */ void Bvector::write(FILE *dat) const { if(dat == stderr || dat == stdout) { fprintf(dat, "Elements: %d\n", nel); for(int i=0; i<=nel-1; ++i) fprintf(dat, "%d ", ((int)m[i])); fprintf(dat, "\n"); } else { comment("nel", dat); fputint(nel, dat); comment("[]", dat); for(int i=0; i<=nel-1; ++i) fputint(((int)m[i]), dat); } return; } /* output to MATLAB script file 'name.m' */ void Bvector::mfile(const char *name) const { FILE *f; int l, i; char n[20]; n[0] = 'v'; n[1] = '_'; l=2; while(l<=15 && name[l-2] != 0) {n[l] = name[l-2]; ++l;} n[l] = '.'; ++l; n[l] = 'm'; ++l; n[l] = 0; fprintf(stderr, "(%d) >> %s\n", nel, n); f = fopen(n, "w+"); fprintf(f, "%s = [\n", name); for(i=0; i<=nel-1; ++i) fprintf(f, "%d\n", ((int)m[i])); fprintf(f, "];\n"); fclose(f); return; } /* input from FILE dat */ void Bvector::read(FILE *dat) { if(dat == stderr || dat == stdout) { matrixerror("Ivector: read"); } else { if(m != NULL) delete[] m; nel = fgetint(dat); m = new bool[nel]; for(int i=0; i<=nel-1; ++i) m[i] = (bool)fgetint(dat); } return; } /* extract a subvector: n entries starting at position x */ Bvector Bvector::subvector(int n, int x) const { if(x < 0 || x >= nel) matrixerror("Bvector: subvector: dimensions, (1)"); if(n < 0) matrixerror("Bvector: subvector: dimensions, (2)"); if(x+n > nel) matrixerror("Bvector: subvector: dimensions, (3)"); Bvector s(n); for(int i=0; i<=n-1; ++i) s(i) = (*this)(x+i); return s; } /* set the entries beginning at position x, y to the values of the supplied (smaller) matrix */ void Bvector::setsubvector(const Bvector &s, int x) { if(x < 0 || x >= nel) matrixerror("Ivector: setsubvector: dimensions, (1)"); if(x+s.nel > nel) matrixerror("Ivector: setsubvector: dimensions, (2)"); for(int i=0; i<=s.nel-1; ++i) (*this)(x+i) = s(i); return; } /* append an entry to the end of the vector */ void Bvector::append(bool e) { bool *n; n = new bool[nel+1]; bool* np = n; bool* mp = m; for(int i=0; i<=nel-1; ++i) *np++ = *mp++; *np = e; if(m != NULL) delete[] m; m = n; ++nel; return; } /* insert value e at position p, shift the following elements accordingly */ void Bvector::insert(bool e, int p) { bool *n; n = new bool[nel+1]; bool* np = n; bool* mp = m; for(int i=0; i<=p-1; ++i) *np++ = *mp++; *np++ = e; for(int i=p; i<=nel-1; ++i) *np++ = *mp++; if(m != NULL) delete[] m; m = n; ++nel; return; } /* remove the element at position p, shift the following elements accordingly */ void Bvector::remove(int p) { bool *n; n = new bool[nel-1]; bool* np = n; bool* mp = m; for(int i=0; i<=p-1; ++i) *np++ = *mp++; ++mp; for(int i=p+1; i<=nel-1; ++i) *np++ = *mp++; if(m != NULL) delete[] m; m = n; --nel; return; } /* ------------------------------------------------------------------- */ /* Cmatrix: twodimensional complex array */ /* initialize */ Cmatrix::Cmatrix() : r(0), c(0), dim(0) { m = NULL; } Cmatrix::Cmatrix(int rows, int columns) : r(rows), c(columns), dim(rows*columns) { m = new Complex[dim]; #ifdef INITARR init(CC0); #endif } Cmatrix::Cmatrix(const Dmatrix& rp, const Dmatrix& ip) : r(rp.r), c(rp.c), dim(rp.r*rp.c) { if(rp.r != ip.r || rp.c != ip.c) matrixerror("Cmatrix(Dmatrix, Dmatrix): dimensions"); m = new Complex[dim]; int yr; for(int y=0; y<=c-1; ++y) { yr = y*r; for(int x=0; x<=r-1; ++x) m[x+yr] = Complex(rp(x,y), ip(x,y)); } } /* destroy */ Cmatrix::~Cmatrix() { if(m != NULL) delete[] m; m = NULL; r = c = dim = 0; } /* copy constructor */ Cmatrix::Cmatrix(const Cmatrix& s) : r(s.r), c(s.c), dim(s.dim) { m = new Complex[dim]; Complex* sp = s.m; Complex* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = *sp++; } /* assignment */ Cmatrix& Cmatrix::operator=(const Cmatrix& s) { if(this != &s) { if(m != NULL) delete[] m; r = s.r; c = s.c; dim = s.dim; m = new Complex[dim]; Complex* sp = s.m; Complex* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = *sp++; } return *this; } /* free allocated memory */ void Cmatrix::strip() { r = 0; c = 0; dim = 0; if(m != NULL) delete[] m; m = NULL; } /* set all elements to d */ void Cmatrix::init(Complex c) { Complex* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = c; return; } /* the complex conjugate */ void Cmatrix::conj() { Complex* mp = m; for(int i=0; i<=dim-1; ++i) { *mp = (*mp).conj(); mp++;} return; } /* the n x n unit matrix */ void Cmatrix::unit(int n) { if(m != NULL) delete[] m; r = n; c = n; dim = r*c; m = new Complex[dim]; init(CC0); for(int i=0; i<=r-1; ++i) (*this)(i, i) = CC1; return; } /* the diagonal matrix with diagonal elements d */ void Cmatrix::diag(Cvector d) { if(m != NULL) delete[] m; r = d.nel; c = d.nel; dim = r*c; m = new Complex[dim]; init(CC0); for(int i=0; i<=r-1; ++i) (*this)(i, i) = d(i); return; } /* extract row x */ Cvector Cmatrix::row(int x) const { Cvector v(c); int ir = x; for(int i=0; i<=c-1; ++i) { v(i) = m[ir]; ir += r; } return v; } /* extract a column */ Cvector Cmatrix::col(int y) const { Cvector v(r); int yr = y*r; for(int i=0; i<=r-1; ++i) { v(i) = m[yr]; ++yr; } return v; } /* set row x */ void Cmatrix::row(Cvector v, int x) { if(v.nel != c) matrixerror("Cmatrix: set row: dimensions"); int ir = x; for(int i=0; i<=c-1; ++i) { m[ir] = v(i); ir += r; } return; } /* set a column */ void Cmatrix::col(Cvector v, int y) { if(v.nel != r) matrixerror("Cmatrix: set column: dimensions"); int yr = y*r; for(int i=0; i<=r-1; ++i) { m[yr] = v(i); ++yr; } return; } /* output to FILE dat */ void Cmatrix::write(FILE *dat) const { if(dat == stderr || dat == stdout) { fprintf(dat, "Rows: %d, Columns: %d, Dim: %d\n", r, c, dim); for(int i=0; i<=r-1; ++i) { for(int j=0; j<=c-1; ++j) { fprintf(dat, "[%d, %d]: ", i, j); ((*this)(i,j)).write(dat); } fprintf(dat, "\n"); } } else { comment("r", dat); fputint(r, dat); comment("c", dat); fputint(c, dat); comment("[][]", dat); for(int i=0; i<=dim-1; ++i) m[i].write(dat); } return; } /* output to MATLAB script file 'name.m' */ void Cmatrix::mfile(const char *name) const { FILE *f; int l; char n[20]; n[0] = 'm'; n[1] = '_'; l=2; while(l<=15 && name[l-2] != 0) {n[l] = name[l-2]; ++l;} n[l] = '.'; ++l; n[l] = 'm'; ++l; n[l] = 0; fprintf(stderr, "(%d, %d) >> %s\n", r, c, n); f = fopen(n, "w+"); fprintf(f, "%s = [\n", name); for(int x=0; x<=r-1; ++x) { for(int y=0; y<=c-2; ++y) fprintf(f, "%.15g+i*%.15g ", m[x+y*r].re, m[x+y*r].im); if(c>=1) fprintf(f, "%.15g+i*%.15g; \n", m[x+(c-1)*r].re, m[x+(c-1)*r].im); } fprintf(f, "];\n"); fclose(f); return; } /* input from FILE dat */ void Cmatrix::read(FILE *dat) { if(dat == stderr || dat == stdout) { matrixerror("Cmatrix: read"); } else { if(m != NULL) delete[] m; r = fgetint(dat); c = fgetint(dat); dim = r*c; m = new Complex[dim]; for(int i=0; i<=dim-1; ++i) m[i].read(dat); } return; } /* real part */ Dmatrix Cmatrix::re() const { Dmatrix nm(r, c); double *np = nm.m; Complex *mp = m; for(int j=0; j<=dim-1; ++j) *np++ = (*mp++).re; return nm; } /* imaginary part */ Dmatrix Cmatrix::im() const { Dmatrix nm(r, c); double *np = nm.m; Complex *mp = m; for(int j=0; j<=dim-1; ++j) *np++ = (*mp++).im; return nm; } /* absolute values of the elements */ Dmatrix Cmatrix::abs() const { Dmatrix nm(r, c); double *np = nm.m; Complex *mp = m; for(int j=0; j<=dim-1; ++j) *np++ = (*mp++).abs(); return nm; } /* squared absolute values of the elements */ Dmatrix Cmatrix::sqabs() const { Dmatrix nm(r, c); double *np = nm.m; Complex *mp = m; for(int j=0; j<=dim-1; ++j) *np++ = (*mp++).sqabs(); return nm; } /* the arguments (phase) of the original entries */ Dmatrix Cmatrix::arg() const { Dmatrix nm(r, c); double *np = nm.m; Complex *mp = m; for(int j=0; j<=dim-1; ++j) *np++ = (*mp++).arg(); return nm; } /* transpose */ void Cmatrix::transpose() { if(m == NULL) return; Complex *nm; nm = new Complex[dim]; int yr, xc; for(int y=0; y<=c-1; ++y) { yr = y*r; for(int x=0; x<=r-1; ++x) { xc = x*c; nm[y+xc] = m[x+yr]; } } yr = c; c = r; r = yr; delete[] m; m = nm; return; } /* adjoint */ void Cmatrix::adjoint() { if(m == NULL) return; Complex *nm; nm = new Complex[dim]; int yr = 0, xc; for(int y=0; y<=c-1; ++y) { xc = 0; for(int x=0; x<=r-1; ++x) { nm[y+xc] = m[x+yr].conj(); xc += c; } yr += r; } yr = c; c = r; r = yr; delete[] m; m = nm; return; } #ifdef INC_EIGEN /* compute the inverse matrix based on the Eigen.inverse() routine */ void Cmatrix::inverse() { if(c != r) matrixerror("Cmatrix: inverse: dimensions"); E_Cmatrix M; M = convEo(*this); M = M.inverse(); (*this) = convEo(M); return; } #else /* compute the inverse matrix Gauss-Jordan-Elimination, based on 'gaussj.c' (C) Copr. 1986-92 Numerical Recipes Software 5.)13. */ void Cmatrix::inverse() { if(c != r) matrixerror("Cmatrix: inverse: dimensions"); int i, icol, irow, j, k, l, ll; Complex dum, pivinv, t; double big; Ivector indxc(r); Ivector indxr(r); Ivector ipiv(r); ipiv.init(0); icol = 0; irow = 0; for(i=0;i<=r-1;i++) { big=0.0; for(j=0;j<=r-1;j++) { if(ipiv(j) != 1) { int jkr = j; for(k=0;k<=r-1;k++) { if(ipiv(k) == 0) { double tb = m[jkr].abs(); if(tb >= big) { big=tb; irow=j; icol=k; } } else { if(ipiv(k) > 1) matrixerror("Cmatrix: inverse: gaussj: singular matrix, 1"); } jkr += r; } } } ++(ipiv(icol)); if(irow != icol) { int irlr = irow, iclr = icol; for(l=0;l<=r-1;l++) { t = m[irlr]; m[irlr] = m[iclr]; m[iclr] = t; irlr += r; iclr += r; } } indxr(i)=irow; indxc(i)=icol; int ccr = icol+icol*r; if(m[ccr].re == 0.0 && m[ccr].im == 0.0) matrixerror("Cmatrix: inverse: gaussj: singular matrix, 2"); pivinv=1.0/m[ccr]; m[ccr]=CC1; for(l=0;l<=r-1;l++) m[icol+l*r] *= pivinv; for(ll=0;ll<=r-1;ll++) if(ll != icol) { dum=m[ll+icol*r]; m[ll+icol*r]=0.0; int lllr = ll, iclr = icol; for(l=0;l<=r-1;l++) { m[lllr] -= m[iclr]*dum; lllr += r; iclr += r; } } } for(l=r-1;l>=0;l--) { if(indxr(l) != indxc(l)) { int irlr = indxr(l)*r, iclr = indxc(l)*r; for(k=0;k<=r-1;k++) { t = m[irlr]; m[irlr] = m[iclr]; m[iclr] = t; ++irlr; ++iclr; } } } return; } #endif /* Cholesky-decomposition of a Hermitian, positive matrix based on: "choldc.c" (C) Copr. 1986-92 Numerical Recipes Software 5.)13. return value: 0 - okay, 1 - matrix is numerically not positive */ int Cmatrix::choldc() { Complex sum; if(r != c) matrixerror("Cmatrix: choldc: non square matrix"); int jr, kr, ir; Dvector p(r); ir = 0; for(int i=0; i<=r-1; ++i) { jr = i*r; for(int j=i; j<=r-1; ++j) { sum = m[i+jr]; kr = (i-1)*r; for(int k=i-1; k>=0; k--) { sum -= (m[i+kr]).conj()*m[j+kr]; kr -= r; } if(i==j) { if(sum.re<=0.0) return 1; p(i) = sqrt(sum.re); } else m[j+ir] = sum/p(i); jr += r; } ir += r; } for(int i=0; i<=r-1; ++i) { jr = 0; for(int j=0; j<=i-1; ++j) { m[i+jr] = m[i+jr].conj(); jr += r; } m[i+jr] = Complex(p(i), 0.0); jr += r; for(int j=i+1; j<=r-1; ++j) { m[i+jr] = CC0; jr += r; } } return 0; } /* backward substitution following Cholesky-decomposition */ /* calculate x as a solution of this * x = rhs, r x r - matrix this, left-triangular, r - dim vector x, returned, r - dim vector rhs */ Cvector Cmatrix::bwdsubst(const Cvector& rhs) { Cvector x(r); int i, k, ir, kr; Complex sum; ir = 0; for(i=0; i<=r-1; ++i) { sum = rhs.m[i]; kr = ir-r; for(k=i-1; k>=0; --k) { sum -= m[i+kr]*x.m[k]; kr -= r; } x.m[i] = sum/m[i+ir]; ir += r; } return x; } /* backward substitution following Cholesky-decomposition */ /* calculate vector a as a solution of this^t * x = rhs, r x r - matrix this, left-triangular, r - dim vector x, returned, r - dim vector rhs */ Cvector Cmatrix::bwdsubst_t(const Cvector& rhs) { Cvector x(r); int i, k, ir; Complex sum; ir = (r-1)*r; for(i=r-1; i>=0; --i) { sum = rhs.m[i]; for(k=i+1; k<=r-1; ++k) sum -= (m[k+ir].conj())*x.m[k]; x.m[i] = sum/m[i+ir]; ir -= r; } return x; } /* solve a linear system with positive matrix by Cholesky decomposition, calculate vector x as a solution of this * x = rhs, r x r - matrix this, positive, destroyed, replaced by a factor of the Cholesky decomposition r - dim vector x, returned, r - dim vector rhs */ Cvector Cmatrix::CDsolve(const Cvector& rhs) { Cvector x, y; int p; p = choldc(); if(p != 0) matrixerror("Cmatrix.CDsolve: matrix not positive"); y = bwdsubst(rhs); x = bwdsubst_t(y); return x; } /* extract a submatrix: nr rows and nc columns starting at position x, y */ Cmatrix Cmatrix::submatrix(int nr, int nc, int x, int y) const { if(x < 0 || x >= r) matrixerror("Cmatrix: submatrix: dimensions, (1)"); if(y < 0 || y >= c) matrixerror("Cmatrix: submatrix: dimensions, (2)"); if(nr < 0 || nc < 0) matrixerror("Cmatrix: submatrix: dimensions, (3)"); if(x+nr > r || y+nc > c) matrixerror("Cmatrix: submatrix: dimensions, (4)"); Cmatrix s(nr, nc); Complex *mp; Complex *sp = s.m; for(int j=0; j<=nc-1; ++j) { mp = &(m[x+(y+j)*r]); for(int i=0; i<=nr-1; ++i) *sp++ = *mp++; } return s; } /* set the entries beginning at position x, y to the values of the supplied (smaller) matrix */ void Cmatrix::setsubmatrix(const Cmatrix &s, int x, int y) { if(x < 0 || x >= r) matrixerror("Cmatrix: setsubmatrix: dimensions, (1)"); if(y < 0 || y >= c) matrixerror("Cmatrix: setsubmatrix: dimensions, (2)"); if(x+s.r > r || y+s.c > c) matrixerror("Cmatrix: setsubmatrix: dimensions, (3)"); Complex *mp; Complex *sp = s.m; for(int j=0; j<=s.c-1; ++j) { mp = &(m[x+(y+j)*r]); for(int i=0; i<=s.r-1; ++i) *mp++ = *sp++; } return; } /* maximum absolute value of the matrix elements, corresponding indices */ double Cmatrix::max() const { double s = m[0].sqabs(); for(int i=1; i<=dim-1; ++i) { if(m[i].sqabs() > s) s = m[i].sqabs(); } return sqrt(s); } double Cmatrix::max(int& x, int& y) const { double s = m[0].sqabs(); int mi = 0; for(int i=1; i<=dim-1; ++i) { if(m[i].sqabs() > s) { s = m[i].sqabs(); mi = i; } } y = mi/r; x = mi-y*r; return sqrt(s); } /* calculate eigenvalues / corresp. eigenvectors of a complex Hermitian matrix, the eigenvalues are returned, the matrix is replaced by its eigenvectors, returned(j) corresponds to the normalized eigenvector (*this).col(j) based on converting the Hermitian eigensystem to a real symmetric system with twice the dimension, see W.H. Press et. al., "Numerical Recipes in C", chapter 11.4 */ Dvector Cmatrix::eigenv() { if(c != r) matrixerror("Cmatrix: eigenv: dimensions"); Dmatrix a, b; a = (*this).re(); b = (*this).im(); Dmatrix m(2*r, 2*r); m.setsubmatrix(a, 0, 0); m.setsubmatrix(mult(b, -1.0), 0, r); m.setsubmatrix(b, r, 0); m.setsubmatrix(a, r, r); Dvector de; de = m.eigenv(); Dvector d(r); Ivector used(2*r); Dvector x, u, v; used.init(0); for(int i=0; i<=r-1; ++i) { double s1 = 1.0e300, s2 = 1.0e300; int si1 = -1, si2 = -1; for(int j=0; j<=2*r-1; ++j) { if(used(j) == 0 && de(j) < s1) { s1 = de(j); si1 = j; } } used(si1) = 1; for(int j=0; j<=2*r-1; ++j) { if(used(j) == 0 && de(j) < s2) { s2 = de(j); si2 = j; } } used(si2) = 1; if(fabs((de(si1)-de(si2))/de(si1)) > 1.0e-8) matrixerror("Cmatrix: eigenv: non-Hermitian matrix"); // fprintf(stderr, "(%d) di = %g, di+1 = %g, diff = %g\n", // i, de(2*i), de(2*i+1), de(2*i)-de(2*i+1)); d(i) = de(si1); x = m.col(si1); u = x.subvector(r, 0); v = x.subvector(r, r); (*this).col(Cvector(u, v), i); } return d; } /* calculate eigenvalues / corresp. eigenvectors of a general complex matrix, the eigenvalues are returned, the matrix is replaced by its eigenvectors, returned(j) corresponds to the normalized eigenvector (*this).col(j) based on the ComplexEigenSolver of the Eigen C++ template library; info: 0: computation successful, 1: erroneous prerequisites, 2: no convergence */ Cvector Cmatrix::geneigenv(int &info) { #ifdef INC_EIGEN if(c != r) matrixerror("Cmatrix: geneigenv: dimensions"); E_Cmatrix M; M = convEo(*this); Eigen::ComplexEigenSolver ces(M, true); Cvector d; info = ces.info(); d = convEo(ces.eigenvalues()); (*this) = convEo(ces.eigenvectors()); return d; #else Cvector d; matrixerror("Cmatrix: geneigenv: Eigen routines not included"); return d; #endif } /* ------------------------------------------------------------------- */ /* Dmatrix: twodimensional double array */ /* initialize */ Dmatrix::Dmatrix() : r(0), c(0), dim(0) { m = NULL; } Dmatrix::Dmatrix(int rows, int columns) : r(rows), c(columns), dim(rows*columns) { m = new double[dim]; #ifdef INITARR init(0.0); #endif } /* destroy */ Dmatrix::~Dmatrix() { if(m != NULL) delete[] m; m = NULL; r = c = dim = 0; } /* copy constructor */ Dmatrix::Dmatrix(const Dmatrix& s) : r(s.r), c(s.c), dim(s.dim) { m = new double[dim]; double* sp = s.m; double* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = *sp++; } /* assignment */ Dmatrix& Dmatrix::operator=(const Dmatrix& s) { if(this != &s) { if(m != NULL) delete[] m; r = s.r; c = s.c; dim = s.dim; m = new double[dim]; double* sp = s.m; double* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = *sp++; } return *this; } /* free allocated memory */ void Dmatrix::strip() { r = 0; c = 0; dim = 0; if(m != NULL) delete[] m; m = NULL; } /* set all elements to d */ void Dmatrix::init(double d) { double* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = d; return; } /* the n x n unit matrix */ void Dmatrix::unit(int n) { if(m != NULL) delete[] m; r = n; c = n; dim = r*c; m = new double[dim]; init(0.0); for(int i=0; i<=r-1; ++i) (*this)(i, i) = 1.0; return; } /* the diagonal matrix with diagonal elements d */ void Dmatrix::diag(Dvector d) { if(m != NULL) delete[] m; r = d.nel; c = d.nel; dim = r*c; m = new double[dim]; init(0.0); for(int i=0; i<=r-1; ++i) (*this)(i, i) = d(i); return; } /* extract row x */ Dvector Dmatrix::row(int x) const { Dvector v(c); int ir = x; for(int i=0; i<=c-1; ++i) { v(i) = m[ir]; ir += r; } return v; } /* extract a column */ Dvector Dmatrix::col(int y) const { Dvector v(r); int yr = y*r; for(int i=0; i<=r-1; ++i) { v(i) = m[yr]; ++yr; } return v; } /* set row x */ void Dmatrix::row(Dvector v, int x) { if(v.nel != c) matrixerror("Dmatrix: set row: dimensions"); int ir = x; for(int i=0; i<=c-1; ++i) { m[ir] = v(i); ir += r; } return; } /* set a column */ void Dmatrix::col(Dvector v, int y) { if(v.nel != r) matrixerror("Dmatrix: set column: dimensions"); int yr = y*r; for(int i=0; i<=r-1; ++i) { m[yr] = v(i); ++yr; } return; } /* output to FILE dat */ void Dmatrix::write(FILE *dat) const { if(dat == stderr || dat == stdout) { fprintf(dat, "Rows: %d, Columns: %d, Dim: %d\n", r, c, dim); for(int i=0; i<=r-1; ++i) { for(int j=0; j<=c-1; ++j) { fprintf(dat, "%g ", (*this)(i,j)); } fprintf(dat, "\n"); } } else { comment("r", dat); fputint(r, dat); comment("c", dat); fputint(c, dat); comment("[][]", dat); for(int i=0; i<=dim-1; ++i) fputdouble(m[i], dat); } return; } /* output to MATLAB script file 'name.m' */ void Dmatrix::mfile(const char *name) const { FILE *f; int l; char n[20]; n[0] = 'm'; n[1] = '_'; l=2; while(l<=15 && name[l-2] != 0) {n[l] = name[l-2]; ++l;} n[l] = '.'; ++l; n[l] = 'm'; ++l; n[l] = 0; fprintf(stderr, "(%d, %d) >> %s\n", r, c, n); f = fopen(n, "w+"); fprintf(f, "%s = [\n", name); for(int x=0; x<=r-1; ++x) { for(int y=0; y<=c-2; ++y) fprintf(f, "%.15g ", m[x+y*r]); if(c>=1) fprintf(f, "%.15g; \n", m[x+(c-1)*r]); } fprintf(f, "];\n"); fclose(f); return; } /* input from FILE dat */ void Dmatrix::read(FILE *dat) { if(dat == stderr || dat == stdout) { matrixerror("Dmatrix: read"); } else { if(m != NULL) delete[] m; r = fgetint(dat); c = fgetint(dat); dim = r*c; m = new double[dim]; for(int i=0; i<=dim-1; ++i) m[i] = fgetdouble(dat); } return; } /* transpose */ void Dmatrix::transpose() { if(m == NULL) return; double *nm; nm = new double[dim]; int yr, xc; for(int y=0; y<=c-1; ++y) { yr = y*r; for(int x=0; x<=r-1; ++x) { xc = x*c; nm[y+xc] = m[x+yr]; } } yr = c; c = r; r = yr; delete[] m; m = nm; return; } /* compute the inverse matrix Gauss-Jordan-Elimination, based on 'gaussj.c' (C) Copr. 1986-92 Numerical Recipes Software 5.)13. */ void Dmatrix::inverse() { if(c != r) matrixerror("Dmatrix: inverse: dimensions"); int i, icol, irow, j, k, l, ll; double dum, pivinv, t; double big; Ivector indxc(r); Ivector indxr(r); Ivector ipiv(r); ipiv.init(0); icol = 0; irow = 0; for(i=0;i<=r-1;i++) { big=0.0; for(j=0;j<=r-1;j++) { if(ipiv(j) != 1) { int jkr = j; for(k=0;k<=r-1;k++) { if(ipiv(k) == 0) { double tb = fabs(m[jkr]); if(tb >= big) { big=tb; irow=j; icol=k; } } else { if(ipiv(k) > 1) matrixerror("Dmatrix: inverse: gaussj: singular matrix, 1"); } jkr += r; } } } ++(ipiv(icol)); if(irow != icol) { int irlr = irow, iclr = icol; for(l=0;l<=r-1;l++) { t = m[irlr]; m[irlr] = m[iclr]; m[iclr] = t; irlr += r; iclr += r; } } indxr(i)=irow; indxc(i)=icol; int ccr = icol+icol*r; if(m[ccr] == 0.0) matrixerror("Dmatrix: inverse: gaussj: singular matrix, 2"); pivinv=1.0/m[ccr]; m[ccr]=1.0; for(l=0;l<=r-1;l++) m[icol+l*r] *= pivinv; for(ll=0;ll<=r-1;ll++) if(ll != icol) { dum=m[ll+icol*r]; m[ll+icol*r]=0.0; int lllr = ll, iclr = icol; for(l=0;l<=r-1;l++) { m[lllr] -= m[iclr]*dum; lllr += r; iclr += r; } } } for(l=r-1;l>=0;l--) { if(indxr(l) != indxc(l)) { int irlr = indxr(l)*r, iclr = indxc(l)*r; for(k=0;k<=r-1;k++) { t = m[irlr]; m[irlr] = m[iclr]; m[iclr] = t; ++irlr; ++iclr; } } } return; } /* Cholesky-decomposition of a symmetrical, positive matrix based on: "choldc.c" (C) Copr. 1986-92 Numerical Recipes Software 5.)13. return value: 0 - okay, 1 - matrix is numerically not positive */ int Dmatrix::choldc() { double sum; if(r != c) matrixerror("Dmatrix: choldc: non square matrix"); Dvector p(r); for(int i=0; i<=r-1; ++i) { for(int j=i; j<=r-1; ++j) { sum = m[i+j*r]; for(int k=i-1; k>=0; k--) sum -= m[i+k*r]*m[j+k*r]; if(i==j) { if(sum<=0.0) return 1; p(i) = sqrt(sum); } else m[j+i*r] = sum/p(i); } } for(int i=0; i<=r-1; ++i) m[i+i*r] = p(i); for(int i=0; i<=r-2; ++i) { for(int j=i+1; j<=r-1; ++j) m[i+j*r] = 0.0; } return 0; } /* backward substitution following Cholesky-decomposition */ /* calculate x as a solution of this * x = rhs, r x r - matrix this, left-triangular, r - dim vector x, returned, r - dim vector rhs */ Dvector Dmatrix::bwdsubst(const Dvector& rhs) { Dvector x(r); int i, k, ir; double sum; ir = 0; for(i=0; i<=r-1; ++i) { sum = rhs.m[i]; for(k=i-1; k>=0; --k) sum -= m[i+k*r]*x.m[k]; x.m[i] = sum/m[i+ir]; ir += r; } return x; } /* backward substitution following Cholesky-decomposition */ /* calculate xmat as a solution of this * xmat = rmat, r x r - matrix this, left-triangular, r x c - matrix xmat, returned, r x c - matrix rmat */ Dmatrix Dmatrix::bwdsubst(const Dmatrix& rmat) { Dmatrix xmat(r, rmat.c); int i, k, l; int ld; int n = rmat.c; double sum; for(l=0; l<=n-1; ++l) { ld = l*r; for(i=0; i<=r-1; ++i) { sum = rmat.m[i+ld]; for(k=i-1; k>=0; --k) sum -= m[i+k*r]*xmat.m[k+ld]; xmat.m[i+ld] = sum/m[i+i*r]; } } return xmat; } /* backward substitution following Cholesky-decomposition */ /* calculate vector a as a solution of this^t * x = rhs, r x r - matrix this, left-triangular, r - dim vector x, returned, r - dim vector rhs */ Dvector Dmatrix::bwdsubst_t(const Dvector& rhs) { Dvector x(r); int i, k, ir; double sum; ir = (r-1)*r; for(i=r-1; i>=0; --i) { sum = rhs.m[i]; for(k=i+1; k<=r-1; ++k) sum -= m[k+ir]*x.m[k]; x.m[i] = sum/m[i+ir]; ir -= r; } return x; } /* solve a linear system with positive matrix by Cholesky decomposition, calculate vector x as a solution of this * x = rhs, r x r - matrix this, positive, destroyed, replaced by a factor of the Chol. dec. r - dim vector x, returned, r - dim vector rhs */ Dvector Dmatrix::CDsolve(const Dvector& rhs) { Dvector x, y; int p; p = choldc(); if(p != 0) matrixerror("Dmatrix.CDsolve: matrix not positive"); y = bwdsubst(rhs); x = bwdsubst_t(y); return x; } /* helper routine */ double dpyth(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)); } /* singular value decomposition based on: "svdcmp.c", "dpythag.c" (C) Copr. 1986-92 Numerical Recipes Software 5.)13. "this"=A is replaced by a matrix U, with a matrix v and a vector w set, such that A=U*diag(w)*v^T */ void Dmatrix::svd(Dvector& w, Dmatrix& v) { int m = r; int n = c; w = Dvector(n); v = Dmatrix(n, n); /* Dmatrix a = (*this); */ int flag, i, its, j, jj, k, l=1, nm=1; double anorm, ct, f, g, h, s, scale, x, y, z; double t; int ti; Dvector rv1(n+1); g=scale=anorm=0.0; for (i=1;i<=n;i++) { l=i+1; rv1(i)=scale*g; g=s=scale=0.0; if (i <= m) { for (k=i;k<=m;k++) scale += fabs((*this)(k-1, i-1)); if (scale) { for (k=i;k<=m;k++) { (*this)(k-1, i-1) /= scale; s += (*this)(k-1, i-1)*(*this)(k-1, i-1); } f=(*this)(i-1, i-1); g = -((f) >= 0.0 ? fabs(sqrt(s)) : -fabs(sqrt(s))); h=f*g-s; (*this)(i-1, i-1)=f-g; for (j=l;j<=n;j++) { for (s=0.0,k=i;k<=m;k++) s += (*this)(k-1, i-1)*(*this)(k-1, j-1); f=s/h; for (k=i;k<=m;k++) (*this)(k-1, j-1) += f*(*this)(k-1, i-1); } for (k=i;k<=m;k++) (*this)(k-1, i-1) *= scale; } } w(i-1)=scale *g; g=s=scale=0.0; if (i <= m && i != n) { for (k=l;k<=n;k++) scale += fabs((*this)(i-1, k-1)); if (scale) { for (k=l;k<=n;k++) { (*this)(i-1, k-1) /= scale; s += (*this)(i-1, k-1)*(*this)(i-1, k-1); } f=(*this)(i-1, l-1); g = -((f) >= 0.0 ? fabs(sqrt(s)) : -fabs(sqrt(s))); h=f*g-s; (*this)(i-1, l-1)=f-g; for (k=l;k<=n;k++) rv1(k)=(*this)(i-1, k-1)/h; for (j=l;j<=m;j++) { for (s=0.0,k=l;k<=n;k++) s += (*this)(j-1, k-1)*(*this)(i-1, k-1); for (k=l;k<=n;k++) (*this)(j-1, k-1) += s*rv1(k); } for (k=l;k<=n;k++) (*this)(i-1, k-1) *= scale; } } t = fabs(w(i-1))+fabs(rv1(i)); if(t > anorm) anorm = t; } for (i=n;i>=1;i--) { if (i < n) { if (g) { for (j=l;j<=n;j++) v(j-1, i-1)=((*this)(i-1, j-1)/(*this)(i-1, l-1))/g; for (j=l;j<=n;j++) { for (s=0.0,k=l;k<=n;k++) s += (*this)(i-1, k-1)*v(k-1, j-1); for (k=l;k<=n;k++) v(k-1, j-1) += s*v(k-1, i-1); } } for (j=l;j<=n;j++) v(i-1, j-1)=v(j-1, i-1)=0.0; } v(i-1, i-1)=1.0; g=rv1(i); l=i; } ti = m; if(n=1;i--) { l=i+1; g=w(i-1); for (j=l;j<=n;j++) (*this)(i-1, j-1)=0.0; if (g) { g=1.0/g; for (j=l;j<=n;j++) { for (s=0.0,k=l;k<=m;k++) s += (*this)(k-1, i-1)*(*this)(k-1, j-1); f=(s/(*this)(i-1, i-1))*g; for (k=i;k<=m;k++) (*this)(k-1, j-1) += f*(*this)(k-1, i-1); } for (j=i;j<=m;j++) (*this)(j-1, i-1) *= g; } else for (j=i;j<=m;j++) (*this)(j-1, i-1)=0.0; ++(*this)(i-1, i-1); } for (k=n;k>=1;k--) { for (its=1;its<=30;its++) { flag=1; for (l=k;l>=1;l--) { nm=l-1; if ((double)(fabs(rv1(l))+anorm) == anorm) { flag=0; break; } if ((double)(fabs(w(nm-1))+anorm) == anorm) break; } if (flag) { ct=0.0; s=1.0; for (i=l;i<=k;i++) { f=s*rv1(i); rv1(i)=ct*rv1(i); if ((double)(fabs(f)+anorm) == anorm) break; g=w(i-1); h=dpyth(f,g); w(i-1)=h; h=1.0/h; ct=g*h; s = -f*h; for (j=1;j<=m;j++) { y=(*this)(j-1, nm-1); z=(*this)(j-1, i-1); (*this)(j-1, nm-1)=y*ct+z*s; (*this)(j-1, i-1)=z*ct-y*s; } } } z=w(k-1); if (l == k) { if (z < 0.0) { w(k-1) = -z; for (j=1;j<=n;j++) v(j-1, k-1) = -v(j-1, k-1); } break; } if (its == 30) matrixerror( "Dmatrix.svd: No convergence in 30 iterations"); x=w(l-1); nm=k-1; y=w(nm-1); g=rv1(nm); h=rv1(k); f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y); g=dpyth(f,1.0); f=((x-z)*(x+z)+h*((y/(f+((f) >= 0.0 ?fabs(g) :-fabs(g))))-h))/x; ct=s=1.0; for (j=l;j<=nm;j++) { i=j+1; g=rv1(i); y=w(i-1); h=s*g; g=ct*g; z=dpyth(f,h); rv1(j)=z; ct=f/z; s=h/z; f=x*ct+g*s; g = g*ct-x*s; h=y*s; y *= ct; for (jj=1;jj<=n;jj++) { x=v(jj-1, j-1); z=v(jj-1, i-1); v(jj-1, j-1)=x*ct+z*s; v(jj-1, i-1)=z*ct-x*s; } z=dpyth(f,h); w(j-1)=z; if (z) { z=1.0/z; ct=f*z; s=h*z; } f=ct*g+s*y; x=ct*y-s*g; for (jj=1;jj<=m;jj++) { y=(*this)(jj-1, j-1); z=(*this)(jj-1, i-1); (*this)(jj-1, j-1)=y*ct+z*s; (*this)(jj-1, i-1)=z*ct-y*s; } } rv1(l)=0.0; rv1(k)=f; w(k-1)=x; } } rv1.strip(); /* Dmatrix vt; vt = v; vt.transpose(); Dmatrix c; c = mult((*this), diagmult(w, vt)); Dmatrix d; d = sub(a, c); double dif, maxd = 0.0; for(int ri=0; ri<=a.r-1; ++ri) { for(int ci=0; ci<=a.c-1; ++ci) { if(a(ri, ci) != 0.0) { // fprintf(stderr, "(%d, %d), %g | %g --> %.13g\n", // ri, ci, a(ri, ci), c(ri, ci), fabs(a(ri, ci)-c(ri, ci))/a(ri, ci)); dif = fabs((a(ri, ci)-c(ri, ci))/a(ri, ci)); if(dif > maxd) maxd = dif; } else { dif = fabs(c(ri, ci)); if(dif > maxd) maxd = dif; } } } fprintf(stderr, "svderr: %.g\n", maxd); */ return; } /* calculate eigenvalues / corresp. eigenvectors of a real symmetric matrix, based on: "tred2.c", "tqli.c", "dpythag.c" (C) Copr. 1986-92 Numerical Recipes Software 5.)13. Only the upper (right) part has to be supplied, the eigenvalues are returned, the matrix is replaced by its eigenvectors, returned(j) corresponds to the normalized eigenvector (*this).col(j) */ Dvector Dmatrix::eigenv() { int o, l, k, j, i; double scale, hh, h, g, f=0.0, p_s, p_r, p_p, p_dd, p_c, p_b; int iter, idim, kdim, ldim, jdim; if(r != c) matrixerror("Dmatrix: eigenv: non square matrix"); Dvector e(r); Dvector d(r); for(i=1; i<=r-1; ++i) { idim = i*r; for(j=0; j<=i-1; ++j) m[i+j*r] = m[j+idim]; } for(i=r-1; i>=1; i--) { l=i-1; ldim = l*r; h=scale=0.0; if(l > 0) { for(k=0; k<=l; k++) scale += fabs(m[i+k*r]); if(scale == 0.0) e(i)=m[i+ldim]; else { for(k=0; k<=l; k++) { kdim = k*r; m[i+kdim] /= scale; h += m[i+kdim]*m[i+kdim]; } f=m[i+ldim]; g=(f >= 0.0 ? -sqrt(h) : sqrt(h)); e(i)=scale*g; h -= f*g; m[i+ldim]=f-g; f=0.0; for(j=0;j<=l;j++) { jdim = j*r; m[j+i*r]=m[i+jdim]/h; g=0.0; for(k=0;k<=j;k++) g += m[j+k*r] *m[i+k*r]; for(k=j+1;k<=l;k++) g += m[k+jdim] *m[i+k*r]; e(j)=g/h; f += e(j)*m[i+jdim]; } hh=f/(h+h); for(j=0; j<=l; j++) { f=m[i+j*r]; e(j)=g=e(j)-hh*f; for(k=0;k<=j;k++) m[j+k*r] -= (f*e(k)+g*m[i+k*r]); } } } else e(i)=m[i+ldim]; d(i)=h; } d(0)=0.0; e(0)=0.0; for(i=0; i<=r-1; i++) { idim = i*r; l=i-1; if(d(i)) { for(j=0; j<=l; j++) { jdim = j*r; g=0.0; for(k=0; k<=l; k++) g += m[i+k*r]*m[k+jdim]; for(k=0; k<=l; k++) m[k+jdim] -= g*m[k+idim]; } } d(i)=m[i+idim]; m[i+idim]=1.0; for(j=0; j<=l; j++) m[j+idim]=m[i+j*r]=0.0; } for(i=1; i<=r-1; i++) e(i-1)=e(i); e(r-1)=0.0; for(l=0; l<=r-1; l++) { iter=0; do { for(o=l+1; o<=r-1; o++) { p_dd=fabs(d(o-1))+fabs(d(o)); if((double)(fabs(e(o-1))+p_dd) == p_dd) break; } if(o != l+1) { if(iter++ == 30) matrixerror("Dmatrix: eigenv: iteration count"); g=(d(l+1)-d(l))/(2.0*e(l)); p_r=dpyth(g,1.0); g=d(o-1)-d(l) +e(l)/(g + (g>=0.0 ? fabs(p_r) : -fabs(p_r))); p_s=p_c=1.0; p_p=0.0; for(i=o-1; i>=l+1; i--) { f=p_s*e(i-1); p_b=p_c*e(i-1); e(i)=(p_r=dpyth(f,g)); if(p_r == 0.0) { d(i) -= p_p; e(o-1)=0.0; break; } p_s=f/p_r; p_c=g/p_r; g=d(i)-p_p; p_r=(d(i-1)-g)*p_s+2.0*p_c*p_b; d(i)=g+(p_p=p_s*p_r); g=p_c*p_r-p_b; for(k=0; k<=r-1; k++) { f=m[k+i*r]; m[k+i*r] =p_s*m[k+(i-1)*r]+p_c*f; m[k+(i-1)*r] =p_c*m[k+(i-1)*r]-p_s*f; } } if(p_r == 0.0 && i >= l+1) continue; d(l) -= p_p; e(l)=g; e(o-1)=0.0; } } while(o != l+1); } return d; } /* extract a submatrix: nr rows and nc columns starting at position x, y */ Dmatrix Dmatrix::submatrix(int nr, int nc, int x, int y) const { if(x < 0 || x >= r) matrixerror("Dmatrix: submatrix: dimensions, (1)"); if(y < 0 || y >= c) matrixerror("Dmatrix: submatrix: dimensions, (2)"); if(nr < 0 || nc < 0) matrixerror("Dmatrix: submatrix: dimensions, (3)"); if(x+nr > r || y+nc > c) matrixerror("Dmatrix: submatrix: dimensions, (4)"); Dmatrix s(nr, nc); double *mp; double *sp = s.m; for(int j=0; j<=nc-1; ++j) { mp = &(m[x+(y+j)*r]); for(int i=0; i<=nr-1; ++i) *sp++ = *mp++; } return s; } /* set the entries beginning at position x, y to the values of the supplied (smaller) matrix */ void Dmatrix::setsubmatrix(const Dmatrix &s, int x, int y) { if(x < 0 || x >= r) matrixerror("Dmatrix: setsubmatrix: dimensions, (1)"); if(y < 0 || y >= c) matrixerror("Dmatrix: setsubmatrix: dimensions, (2)"); if(x+s.r > r || y+s.c > c) matrixerror("Dmatrix: setsubmatrix: dimensions, (3)"); double *mp; double *sp = s.m; for(int j=0; j<=s.c-1; ++j) { mp = &(m[x+(y+j)*r]); for(int i=0; i<=s.r-1; ++i) *mp++ = *sp++; } return; } /* minimum and maximum of the matrix elements, corresponding indices */ double Dmatrix::min() const { double s = m[0]; for(int i=1; i<=dim-1; ++i) { if(m[i] < s) s = m[i]; } return s; } double Dmatrix::min(int& x, int& y) const { double s = m[0]; int mi = 0; for(int i=1; i<=dim-1; ++i) { if(m[i] < s) { s = m[i]; mi = i; } } y = mi/r; x = mi-y*r; return s; } double Dmatrix::max() const { double s = m[0]; for(int i=1; i<=dim-1; ++i) { if(m[i] > s) s = m[i]; } return s; } double Dmatrix::max(int& x, int& y) const { double s = m[0]; int mi = 0; for(int i=1; i<=dim-1; ++i) { if(m[i] > s) { s = m[i]; mi = i; } } y = mi/r; x = mi-y*r; return s; } /* ------------------------------------------------------------------- */ /* Imatrix: twodimensional int array */ /* initialize */ Imatrix::Imatrix() : r(0), c(0), dim(0) { m = NULL; } Imatrix::Imatrix(int rows, int columns) : r(rows), c(columns), dim(rows*columns) { m = new int[dim]; #ifdef INITARR init(0); #endif } /* destroy */ Imatrix::~Imatrix() { if(m != NULL) delete[] m; m = NULL; r = c = dim = 0; } /* copy constructor */ Imatrix::Imatrix(const Imatrix& s) : r(s.r), c(s.c), dim(s.dim) { m = new int[dim]; int* sp = s.m; int* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = *sp++; } /* assignment */ Imatrix& Imatrix::operator=(const Imatrix& s) { if(this != &s) { if(m != NULL) delete[] m; r = s.r; c = s.c; dim = s.dim; m = new int[dim]; int* sp = s.m; int* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = *sp++; } return *this; } /* free allocated memory */ void Imatrix::strip() { r = 0; c = 0; dim = 0; if(m != NULL) delete[] m; m = NULL; } /* set all elements to i */ void Imatrix::init(int i) { int* mp = m; for(int j=0; j<=dim-1; ++j) *mp++ = i; return; } /* the n x n unit matrix */ void Imatrix::unit(int n) { if(m != NULL) delete[] m; r = n; c = n; dim = r*c; m = new int[dim]; init(0); for(int i=0; i<=r-1; ++i) (*this)(i, i) = 1; return; } /* extract row x */ Ivector Imatrix::row(int x) const { Ivector v(c); int ir = x; for(int i=0; i<=c-1; ++i) { v(i) = m[ir]; ir += r; } return v; } /* extract a column */ Ivector Imatrix::col(int y) const { Ivector v(r); int yr = y*r; for(int i=0; i<=r-1; ++i) { v(i) = m[yr]; ++yr; } return v; } /* set row x */ void Imatrix::row(Ivector v, int x) { if(v.nel != c) matrixerror("Imatrix: set row: dimensions"); int ir = x; for(int i=0; i<=c-1; ++i) { m[ir] = v(i); ir += r; } return; } /* set a column */ void Imatrix::col(Ivector v, int y) { if(v.nel != r) matrixerror("Imatrix: set column: dimensions"); int yr = y*r; for(int i=0; i<=r-1; ++i) { m[yr] = v(i); ++yr; } return; } /* output to FILE dat */ void Imatrix::write(FILE *dat) const { if(dat == stderr || dat == stdout) { fprintf(dat, "Rows: %d, Columns: %d, Dim: %d\n", r, c, dim); for(int i=0; i<=r-1; ++i) { for(int j=0; j<=c-1; ++j) fprintf(dat, "%d ", (*this)(i,j)); fprintf(dat, "\n"); } } else { comment("r", dat); fputint(r, dat); comment("c", dat); fputint(c, dat); comment("[][]", dat); for(int i=0; i<=dim-1; ++i) fputint(m[i], dat); } return; } /* output to MATLAB script file 'name.m' */ void Imatrix::mfile(const char *name) const { FILE *f; int l; char n[20]; n[0] = 'm'; n[1] = '_'; l=2; while(l<=15 && name[l-2] != 0) {n[l] = name[l-2]; ++l;} n[l] = '.'; ++l; n[l] = 'm'; ++l; n[l] = 0; fprintf(stderr, "(%d, %d) >> %s\n", r, c, n); f = fopen(n, "w+"); fprintf(f, "%s = [\n", name); for(int x=0; x<=r-1; ++x) { for(int y=0; y<=c-2; ++y) fprintf(f, "%d ", m[x+y*r]); if(c>=1) fprintf(f, "%d; \n", m[x+(c-1)*r]); } fprintf(f, "];\n"); fclose(f); return; } /* input from FILE dat */ void Imatrix::read(FILE *dat) { if(dat == stderr || dat == stdout) { matrixerror("Imatrix: read"); } else { if(m != NULL) delete[] m; r = fgetint(dat); c = fgetint(dat); dim = r*c; m = new int[dim]; for(int i=0; i<=dim-1; ++i) m[i] = fgetint(dat); } return; } /* transpose */ void Imatrix::transpose() { if(m == NULL) return; int *nm; nm = new int[dim]; int yr, xc; for(int y=0; y<=c-1; ++y) { yr = y*r; for(int x=0; x<=r-1; ++x) { xc = x*c; nm[y+xc] = m[x+yr]; } } yr = c; c = r; r = yr; delete[] m; m = nm; return; } /* minimum and maximum of the matrix elements, corresponding indices */ int Imatrix::min() const { int s = m[0]; for(int i=1; i<=dim-1; ++i) { if(m[i] < s) s = m[i]; } return s; } int Imatrix::min(int& x, int& y) const { int s = m[0]; int mi = 0; for(int i=1; i<=dim-1; ++i) { if(m[i] < s) { s = m[i]; mi = i; } } y = mi/r; x = mi-y*r; return s; } int Imatrix::max() const { int s = m[0]; for(int i=1; i<=dim-1; ++i) { if(m[i] > s) s = m[i]; } return s; } int Imatrix::max(int& x, int& y) const { int s = m[0]; int mi = 0; for(int i=1; i<=dim-1; ++i) { if(m[i] > s) { s = m[i]; mi = i; } } y = mi/r; x = mi-y*r; return s; } /* ------------------------------------------------------------------- */ /* Bmatrix: twodimensional bool array */ /* initialize */ Bmatrix::Bmatrix() : r(0), c(0), dim(0) { m = NULL; } Bmatrix::Bmatrix(int rows, int columns) : r(rows), c(columns), dim(rows*columns) { m = new bool[dim]; #ifdef INITARR init(false); #endif } /* destroy */ Bmatrix::~Bmatrix() { if(m != NULL) delete[] m; m = NULL; r = c = dim = 0; } /* copy constructor */ Bmatrix::Bmatrix(const Bmatrix& s) : r(s.r), c(s.c), dim(s.dim) { m = new bool[dim]; bool* sp = s.m; bool* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = *sp++; } /* assignment */ Bmatrix& Bmatrix::operator=(const Bmatrix& s) { if(this != &s) { if(m != NULL) delete[] m; r = s.r; c = s.c; dim = s.dim; m = new bool[dim]; bool* sp = s.m; bool* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = *sp++; } return *this; } /* free allocated memory */ void Bmatrix::strip() { r = 0; c = 0; dim = 0; if(m != NULL) delete[] m; m = NULL; } /* set all elements to i */ void Bmatrix::init(bool b) { bool* mp = m; for(int j=0; j<=dim-1; ++j) *mp++ = b; return; } /* extract row x */ Bvector Bmatrix::row(int x) const { Bvector v(c); int ir = x; for(int i=0; i<=c-1; ++i) { v(i) = m[ir]; ir += r; } return v; } /* extract a column */ Bvector Bmatrix::col(int y) const { Bvector v(r); int yr = y*r; for(int i=0; i<=r-1; ++i) { v(i) = m[yr]; ++yr; } return v; } /* set row x */ void Bmatrix::row(Bvector v, int x) { if(v.nel != c) matrixerror("Bmatrix: set row: dimensions"); int ir = x; for(int i=0; i<=c-1; ++i) { m[ir] = v(i); ir += r; } return; } /* set a column */ void Bmatrix::col(Bvector v, int y) { if(v.nel != r) matrixerror("Bmatrix: set column: dimensions"); int yr = y*r; for(int i=0; i<=r-1; ++i) { m[yr] = v(i); ++yr; } return; } /* output to FILE dat */ void Bmatrix::write(FILE *dat) const { if(dat == stderr || dat == stdout) { fprintf(dat, "Rows: %d, Columns: %d, Dim: %d\n", r, c, dim); for(int i=0; i<=r-1; ++i) { for(int j=0; j<=c-1; ++j) fprintf(dat, "%d ", (int)(*this)(i,j)); fprintf(dat, "\n"); } } else { comment("r", dat); fputint(r, dat); comment("c", dat); fputint(c, dat); comment("[][]", dat); for(int i=0; i<=dim-1; ++i) fputint((int)m[i], dat); } return; } /* output to MATLAB script file 'name.m' */ void Bmatrix::mfile(const char *name) const { FILE *f; int l; char n[20]; n[0] = 'm'; n[1] = '_'; l=2; while(l<=15 && name[l-2] != 0) {n[l] = name[l-2]; ++l;} n[l] = '.'; ++l; n[l] = 'm'; ++l; n[l] = 0; fprintf(stderr, "(%d, %d) >> %s\n", r, c, n); f = fopen(n, "w+"); fprintf(f, "%s = [\n", name); for(int x=0; x<=r-1; ++x) { for(int y=0; y<=c-2; ++y) fprintf(f, "%d ", ((int)m[x+y*r])); if(c>=1) fprintf(f, "%d; \n", ((int)m[x+(c-1)*r])); } fprintf(f, "];\n"); fclose(f); return; } /* input from FILE dat */ void Bmatrix::read(FILE *dat) { if(dat == stderr || dat == stdout) { matrixerror("Bmatrix: read"); } else { if(m != NULL) delete[] m; r = fgetint(dat); c = fgetint(dat); dim = r*c; m = new bool[dim]; for(int i=0; i<=dim-1; ++i) m[i] = (bool)fgetint(dat); } return; } /* ----------------------------------------------------------------- */ /* multiplications */ Cmatrix mult(const Cmatrix& a, const Cmatrix& b) { if(a.c != b.r) matrixerror("mult: dimensions"); Cmatrix n(a.r, b.c); n.init(CC0); int ra = a.r, rb = b.r, ca = a.c, cb = b.c; int xj = 0, bi; Complex *np, *np0 = n.m; Complex *ap, *ap0 = a.m; Complex bjy; for(int j=0; j<=ca-1; ++j) { np = np0; bi = j; ap0 = &(a.m[xj]); for(int y=0; y<=cb-1; ++y) { bjy = b.m[bi]; ap = ap0; for(int x=0; x<=ra-1; ++x) (*np++) += (*ap++)*bjy; bi += rb; } xj += ra; } return n; } Cmatrix mult(const Cmatrix& a, const Dmatrix& b) { if(a.c != b.r) matrixerror("mult: dimensions"); Cmatrix n(a.r, b.c); n.init(CC0); int ra = a.r, rb = b.r, ca = a.c, cb = b.c; int xj = 0, bi; Complex *np, *np0 = n.m; Complex *ap, *ap0 = a.m; double bjy; for(int j=0; j<=ca-1; ++j) { np = np0; bi = j; ap0 = &(a.m[xj]); for(int y=0; y<=cb-1; ++y) { bjy = b.m[bi]; ap = ap0; for(int x=0; x<=ra-1; ++x) (*np++) += (*ap++)*bjy; bi += rb; } xj += ra; } return n; } Cmatrix mult(const Dmatrix& a, const Cmatrix& b) { if(a.c != b.r) matrixerror("mult: dimensions"); Cmatrix n(a.r, b.c); n.init(CC0); int ra = a.r, rb = b.r, ca = a.c, cb = b.c; int xj = 0, bi; Complex *np, *np0 = n.m; double *ap, *ap0 = a.m; Complex bjy; for(int j=0; j<=ca-1; ++j) { np = np0; bi = j; ap0 = &(a.m[xj]); for(int y=0; y<=cb-1; ++y) { bjy = b.m[bi]; ap = ap0; for(int x=0; x<=ra-1; ++x) (*np++) += bjy*(*ap++); bi += rb; } xj += ra; } return n; } Dmatrix mult(const Dmatrix& a, const Dmatrix& b) { if(a.c != b.r) matrixerror("mult: dimensions"); Dmatrix n(a.r, b.c); n.init(0.0); int ra = a.r, rb = b.r, ca = a.c, cb = b.c; int xj = 0, bi; double *np, *np0 = n.m; double *ap, *ap0 = a.m; double bjy; for(int j=0; j<=ca-1; ++j) { np = np0; bi = j; ap0 = &(a.m[xj]); for(int y=0; y<=cb-1; ++y) { bjy = b.m[bi]; ap = ap0; for(int x=0; x<=ra-1; ++x) (*np++) += (*ap++)*bjy; bi += rb; } xj += ra; } return n; } Cvector mult(const Cmatrix& a, const Cvector& b) { if(a.c != b.nel) matrixerror("mult: dimensions"); Cvector n(a.r); n.init(CC0); int ra = a.r, ca = a.c; Complex *np, *np0 = n.m; Complex *ap = a.m; Complex bj; for(int j=0; j<=ca-1; ++j) { np = np0; bj = b.m[j]; for(int x=0; x<=ra-1; ++x) (*np++) += (*ap++)*bj; } return n; } Cvector mult(const Dmatrix& a, const Cvector& b) { if(a.c != b.nel) matrixerror("mult: dimensions"); Cvector n(a.r); n.init(CC0); int ra = a.r, ca = a.c; Complex *np, *np0 = n.m; double *ap = a.m; Complex bj; for(int j=0; j<=ca-1; ++j) { np = np0; bj = b.m[j]; for(int x=0; x<=ra-1; ++x) (*np++) += bj*(*ap++); } return n; } Cvector mult(const Cmatrix& a, const Dvector& b) { if(a.c != b.nel) matrixerror("mult: dimensions"); Cvector n(a.r); n.init(CC0); int ra = a.r, ca = a.c; Complex *np, *np0 = n.m; Complex *ap = a.m; double bj; for(int j=0; j<=ca-1; ++j) { np = np0; bj = b.m[j]; for(int x=0; x<=ra-1; ++x) (*np++) += (*ap++)*bj; } return n; } Dvector mult(const Dmatrix& a, const Dvector& b) { if(a.c != b.nel) matrixerror("mult: dimensions"); Dvector n(a.r); n.init(0.0); int ra = a.r, ca = a.c; double *np, *np0 = n.m; double *ap = a.m; double bj; for(int j=0; j<=ca-1; ++j) { np = np0; bj = b.m[j]; for(int x=0; x<=ra-1; ++x) (*np++) += (*ap++)*bj; } return n; } Cmatrix mult(const Cmatrix& a, const Complex& b) { Cmatrix n(a.r, a.c); int dim = a.dim; Complex* ap = a.m; Complex* np = n.m; for(int i=0; i<=dim-1; ++i) *np++ = (*ap++)*b; return n; } Cmatrix mult(const Cmatrix& a, double b) { Cmatrix n(a.r, a.c); int dim = a.dim; Complex* ap = a.m; Complex* np = n.m; for(int i=0; i<=dim-1; ++i) *np++ = (*ap++)*b; return n; } Cmatrix mult(const Dmatrix& a, const Complex& b) { Cmatrix n(a.r, a.c); int dim = a.dim; double* ap = a.m; Complex* np = n.m; for(int i=0; i<=dim-1; ++i) *np++ = b*(*ap++); return n; } Dmatrix mult(const Dmatrix& a, double b) { Dmatrix n(a.r, a.c); int dim = a.dim; double* ap = a.m; double* np = n.m; for(int i=0; i<=dim-1; ++i) *np++ = (*ap++)*b; return n; } Cmatrix mult(const Complex& a, const Cmatrix& b) { Cmatrix n(b.r, b.c); int dim = b.dim; Complex* bp = b.m; Complex* np = n.m; for(int i=0; i<=dim-1; ++i) *np++ = a*(*bp++); return n; } Cmatrix mult(const Complex& a, const Dmatrix& b) { Cmatrix n(b.r, b.c); int dim = b.dim; double* bp = b.m; Complex* np = n.m; for(int i=0; i<=dim-1; ++i) *np++ = a*(*bp++); return n; } Cmatrix mult(double a, const Cmatrix& b) { Cmatrix n(b.r, b.c); int dim = b.dim; Complex* bp = b.m; Complex* np = n.m; for(int i=0; i<=dim-1; ++i) *np++ = (*bp++)*a; return n; } Dmatrix mult(double a, const Dmatrix& b) { Dmatrix n(b.r, b.c); int dim = b.dim; double* bp = b.m; double* np = n.m; for(int i=0; i<=dim-1; ++i) *np++ = a*(*bp++); return n; } Cvector mult(const Cvector& a, const Complex& b) { int nel = a.nel; Cvector n(nel); Complex* ap = a.m; Complex* np = n.m; for(int i=0; i<=nel-1; ++i) *np++ = (*ap++)*b; return n; } Cvector mult(const Cvector& a, double b) { int nel = a.nel; Cvector n(nel); Complex* ap = a.m; Complex* np = n.m; for(int i=0; i<=nel-1; ++i) *np++ = (*ap++)*b; return n; } Cvector mult(const Dvector& a, const Complex& b) { int nel = a.nel; Cvector n(nel); double* ap = a.m; Complex* np = n.m; for(int i=0; i<=nel-1; ++i) *np++ = b*(*ap++); return n; } Dvector mult(const Dvector& a, double b) { int nel = a.nel; Dvector n(nel); double* ap = a.m; double* np = n.m; for(int i=0; i<=nel-1; ++i) *np++ = (*ap++)*b; return n; } Cvector mult(const Complex& a, const Cvector& b) { int nel = b.nel; Cvector n(nel); Complex* bp = b.m; Complex* np = n.m; for(int i=0; i<=nel-1; ++i) *np++ = a*(*bp++); return n; } Cvector mult(const Complex& a, const Dvector& b) { int nel = b.nel; Cvector n(nel); double* bp = b.m; Complex* np = n.m; for(int i=0; i<=nel-1; ++i) *np++ = a*(*bp++); return n; } Cvector mult(double a, const Cvector& b) { int nel = b.nel; Cvector n(nel); Complex* bp = b.m; Complex* np = n.m; for(int i=0; i<=nel-1; ++i) *np++ = (*bp++)*a; return n; } Dvector mult(double a, const Dvector& b) { int nel = b.nel; Dvector n(nel); double* bp = b.m; double* np = n.m; for(int i=0; i<=nel-1; ++i) *np++ = a*(*bp++); return n; } void Cmatrix::multeq(const Complex& c) { Complex* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ *= c; return; } void Cmatrix::multeq(double c) { Complex* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ *= c; return; } void Dmatrix::multeq(double c) { double* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ *= c; return; } void Cvector::multeq(const Complex& c) { Complex* mp = m; for(int i=0; i<=nel-1; ++i) *mp++ *= c; return; } void Cvector::multeq(double c) { Complex* mp = m; for(int i=0; i<=nel-1; ++i) *mp++ *= c; return; } void Dvector::multeq(double c) { double* mp = m; for(int i=0; i<=nel-1; ++i) *mp++ *= c; return; } /* additions */ Cmatrix add(const Cmatrix& a, const Cmatrix& b) { if(a.c != b.c || a.r != b.r) matrixerror("add: dimensions"); Cmatrix n(a.r, a.c); int dim = a.dim; Complex* ap = a.m; Complex* bp = b.m; Complex* np = n.m; for(int i=0; i<=dim-1; ++i) *np++ = *ap++ + *bp++; return n; } Cmatrix add(const Cmatrix& a, const Dmatrix& b) { if(a.c != b.c || a.r != b.r) matrixerror("add: dimensions"); Cmatrix n(a.r, a.c); int dim = a.dim; Complex* ap = a.m; double* bp = b.m; Complex* np = n.m; for(int i=0; i<=dim-1; ++i) *np++ = *ap++ + *bp++; return n; } Cmatrix add(const Dmatrix& a, const Cmatrix& b) { if(a.c != b.c || a.r != b.r) matrixerror("add: dimensions"); Cmatrix n(a.r, a.c); int dim = a.dim; double* ap = a.m; Complex* bp = b.m; Complex* np = n.m; for(int i=0; i<=dim-1; ++i) *np++ = *ap++ + *bp++; return n; } Dmatrix add(const Dmatrix& a, const Dmatrix& b) { if(a.c != b.c || a.r != b.r) matrixerror("add: dimensions"); Dmatrix n(a.r, a.c); int dim = a.dim; double* ap = a.m; double* bp = b.m; double* np = n.m; for(int i=0; i<=dim-1; ++i) *np++ = *ap++ + *bp++; return n; } Cvector add(const Cvector& a, const Cvector& b) { if(a.nel != b.nel) matrixerror("add: dimensions"); int nel = a.nel; Cvector n(nel); Complex* ap = a.m; Complex* bp = b.m; Complex* np = n.m; for(int i=0; i<=nel-1; ++i) *np++ = *ap++ + *bp++; return n; } Cvector add(const Dvector& a, const Cvector& b) { if(a.nel != b.nel) matrixerror("add: dimensions"); int nel = a.nel; Cvector n(nel); double* ap = a.m; Complex* bp = b.m; Complex* np = n.m; for(int i=0; i<=nel-1; ++i) *np++ = *ap++ + *bp++; return n; } Cvector add(const Cvector& a, const Dvector& b) { if(a.nel != b.nel) matrixerror("add: dimensions"); int nel = a.nel; Cvector n(nel); Complex* ap = a.m; double* bp = b.m; Complex* np = n.m; for(int i=0; i<=nel-1; ++i) *np++ = *ap++ + *bp++; return n; } Dvector add(const Dvector& a, const Dvector& b) { if(a.nel != b.nel) matrixerror("add: dimensions"); int nel = a.nel; Dvector n(nel); double* ap = a.m; double* bp = b.m; double* np = n.m; for(int i=0; i<=nel-1; ++i) *np++ = *ap++ + *bp++; return n; } void Cmatrix::addeq(const Cmatrix& b) { if(c != b.c || r != b.r) matrixerror("add: dimensions"); Complex* mp = m; Complex* bp = b.m; for(int i=0; i<=dim-1; ++i) *mp++ += *bp++; return; } void Cmatrix::addeq(const Dmatrix& b) { if(c != b.c || r != b.r) matrixerror("add: dimensions"); Complex* mp = m; double* bp = b.m; for(int i=0; i<=dim-1; ++i) *mp++ += *bp++; return; } void Dmatrix::addeq(const Dmatrix& b) { if(c != b.c || r != b.r) matrixerror("add: dimensions"); double* mp = m; double* bp = b.m; for(int i=0; i<=dim-1; ++i) *mp++ += *bp++; return; } void Cvector::addeq(const Cvector& b) { if(nel != b.nel) matrixerror("add: dimensions"); Complex* mp = m; Complex* bp = b.m; for(int i=0; i<=nel-1; ++i) *mp++ += *bp++; return; } void Cvector::addeq(const Dvector& b) { if(nel != b.nel) matrixerror("add: dimensions"); Complex* mp = m; double* bp = b.m; for(int i=0; i<=nel-1; ++i) *mp++ += *bp++; return; } void Dvector::addeq(const Dvector& b) { if(nel != b.nel) matrixerror("add: dimensions"); double* mp = m; double* bp = b.m; for(int i=0; i<=nel-1; ++i) *mp++ += *bp++; return; } /* subtraction */ Cmatrix sub(const Cmatrix& a, const Cmatrix& b) { if(a.c != b.c || a.r != b.r) matrixerror("sub: dimensions"); Cmatrix n(a.r, a.c); int dim = a.dim; Complex* ap = a.m; Complex* bp = b.m; Complex* np = n.m; for(int i=0; i<=dim-1; ++i) *np++ = *ap++ - *bp++; return n; } Cmatrix sub(const Cmatrix& a, const Dmatrix& b) { if(a.c != b.c || a.r != b.r) matrixerror("sub: dimensions"); Cmatrix n(a.r, a.c); int dim = a.dim; Complex* ap = a.m; double* bp = b.m; Complex* np = n.m; for(int i=0; i<=dim-1; ++i) *np++ = *ap++ - *bp++; return n; } Cmatrix sub(const Dmatrix& a, const Cmatrix& b) { if(a.c != b.c || a.r != b.r) matrixerror("sub: dimensions"); Cmatrix n(a.r, a.c); int dim = a.dim; double* ap = a.m; Complex* bp = b.m; Complex* np = n.m; for(int i=0; i<=dim-1; ++i) *np++ = *ap++ - *bp++; return n; } Dmatrix sub(const Dmatrix& a, const Dmatrix& b) { if(a.c != b.c || a.r != b.r) matrixerror("sub: dimensions"); Dmatrix n(a.r, a.c); int dim = a.dim; double* ap = a.m; double* bp = b.m; double* np = n.m; for(int i=0; i<=dim-1; ++i) *np++ = *ap++ - *bp++; return n; } Cvector sub(const Cvector& a, const Cvector& b) { if(a.nel != b.nel) matrixerror("sub: dimensions"); int nel = a.nel; Cvector n(nel); Complex* ap = a.m; Complex* bp = b.m; Complex* np = n.m; for(int i=0; i<=nel-1; ++i) *np++ = *ap++ - *bp++; return n; } Cvector sub(const Dvector& a, const Cvector& b) { if(a.nel != b.nel) matrixerror("sub: dimensions"); int nel = a.nel; Cvector n(nel); double* ap = a.m; Complex* bp = b.m; Complex* np = n.m; for(int i=0; i<=nel-1; ++i) *np++ = *ap++ - *bp++; return n; } Cvector sub(const Cvector& a, const Dvector& b) { if(a.nel != b.nel) matrixerror("sub: dimensions"); int nel = a.nel; Cvector n(nel); Complex* ap = a.m; double* bp = b.m; Complex* np = n.m; for(int i=0; i<=nel-1; ++i) *np++ = *ap++ - *bp++; return n; } Dvector sub(const Dvector& a, const Dvector& b) { if(a.nel != b.nel) matrixerror("sub: dimensions"); int nel = a.nel; Dvector n(nel); double* ap = a.m; double* bp = b.m; double* np = n.m; for(int i=0; i<=nel-1; ++i) *np++ = *ap++ - *bp++; return n; } void Cmatrix::subeq(const Cmatrix& b) { if(c != b.c || r != b.r) matrixerror("sub: dimensions"); Complex* mp = m; Complex* bp = b.m; for(int i=0; i<=dim-1; ++i) *mp++ -= *bp++; return; } void Cmatrix::subeq(const Dmatrix& b) { if(c != b.c || r != b.r) matrixerror("sub: dimensions"); Complex* mp = m; double* bp = b.m; for(int i=0; i<=dim-1; ++i) *mp++ -= *bp++; return; } void Dmatrix::subeq(const Dmatrix& b) { if(c != b.c || r != b.r) matrixerror("sub: dimensions"); double* mp = m; double* bp = b.m; for(int i=0; i<=dim-1; ++i) *mp++ -= *bp++; return; } void Cvector::subeq(const Cvector& b) { if(nel != b.nel) matrixerror("sub: dimensions"); Complex* mp = m; Complex* bp = b.m; for(int i=0; i<=nel-1; ++i) *mp++ -= *bp++; return; } void Cvector::subeq(const Dvector& b) { if(nel != b.nel) matrixerror("sub: dimensions"); Complex* mp = m; double* bp = b.m; for(int i=0; i<=nel-1; ++i) *mp++ -= *bp++; return; } void Dvector::subeq(const Dvector& b) { if(nel != b.nel) matrixerror("sub: dimensions"); double* mp = m; double* bp = b.m; for(int i=0; i<=nel-1; ++i) *mp++ -= *bp++; return; } /* multiplications, the argument d represents the elements of a square diagonal matrix */ Cmatrix diagmult(const Cvector& d, const Cmatrix& m) { if(d.nel != m.r) matrixerror("diagmult: dimensions"); Cmatrix n(d.nel, m.c); int neld = d.nel, cm = m.c; Complex *dp, *dp0 = d.m; Complex *mp = m.m; Complex *np = n.m; for(int y=0; y<=cm-1; ++y) { dp = dp0; for(int x=0; x<=neld-1; ++x) *np++ = (*dp++)*(*mp++); } return n; } Cmatrix diagmult(const Cmatrix& m, const Cvector& d) { if(m.c != d.nel) matrixerror("diagmult: dimensions"); Cmatrix n(m.r, d.nel); int neld = d.nel, rm = m.r; Complex *dp = d.m; Complex *mp = m.m; Complex *np = n.m; Complex s; for(int y=0; y<=neld-1; ++y) { s = *dp++; for(int x=0; x<=rm-1; ++x) *np++ = (*mp++)*s; } return n; } Cvector diagmult(const Cvector& d, const Cvector& a) { if(d.nel != a.nel) matrixerror("diagmult: dimensions"); int nel = d.nel; Cvector n(nel); Complex* dp = d.m; Complex* ap = a.m; Complex* np = n.m; for(int i=0; i<=nel-1; ++i) (*np++) = (*dp++)*(*ap++); return n; } Dmatrix diagmult(const Dvector& d, const Dmatrix& m) { if(d.nel != m.r) matrixerror("diagmult: dimensions"); Dmatrix n(d.nel, m.c); int neld = d.nel, cm = m.c; double *dp, *dp0 = d.m; double *mp = m.m; double *np = n.m; for(int y=0; y<=cm-1; ++y) { dp = dp0; for(int x=0; x<=neld-1; ++x) *np++ = (*dp++)*(*mp++); } return n; } Dmatrix diagmult(const Dmatrix& m, const Dvector& d) { if(m.c != d.nel) matrixerror("diagmult: dimensions"); Dmatrix n(m.r, d.nel); int neld = d.nel, rm = m.r; double *dp = d.m; double *mp = m.m; double *np = n.m; double s; for(int y=0; y<=neld-1; ++y) { s = *dp++; for(int x=0; x<=rm-1; ++x) *np++ = (*mp++)*s; } return n; } Dvector diagmult(const Dvector& d, const Dvector& a) { if(d.nel != a.nel) matrixerror("diagmult: dimensions"); int nel = d.nel; Dvector n(nel); double* dp = d.m; double* ap = a.m; double* np = n.m; for(int i=0; i<=nel-1; ++i) (*np++) = (*dp++)*(*ap++); return n; } /* scalar products */ Complex scalp(const Cvector& a, const Cvector& b) { if(a.nel != b.nel) matrixerror("scalp: dimensions"); Complex s = CC0; int nel = a.nel; Complex* ap = a.m; Complex* bp = b.m; for(int i=0; i<=nel-1; ++i) s += (*ap++).conj()*(*bp++); return s; } Complex scalp(const Cvector& a, const Dvector& b) { if(a.nel != b.nel) matrixerror("scalp: dimensions"); Complex s = CC0; int nel = a.nel; Complex* ap = a.m; double* bp = b.m; for(int i=0; i<=nel-1; ++i) s += (*ap++).conj()*(*bp++); return s; } Complex scalp(const Dvector& a, const Cvector& b) { if(a.nel != b.nel) matrixerror("scalp: dimensions"); Complex s = CC0; int nel = a.nel; double* ap = a.m; Complex* bp = b.m; for(int i=0; i<=nel-1; ++i) s += (*ap++)*(*bp++); return s; } double scalp(const Dvector& a, const Dvector& b) { if(a.nel != b.nel) matrixerror("scalp: dimensions"); double s=0.0; int nel = a.nel; double* ap = a.m; double* bp = b.m; for(int i=0; i<=nel-1; ++i) s += (*ap++)*(*bp++); return s; } /* ------------------------------------------------------------------- */ /* solve the real eigenvalue problem a*x = lambda*x for all eigenvalues lambda and eigenvector x (columns of x), ordered, lambda(i) decreases with i */ void solveEVproblem(Dmatrix& a, Dvector& lambda, Dmatrix& x) { int i, k, im; double lm; Dvector t(a.r); x = a; lambda = x.eigenv(); for(i=0; i<=a.r-2; ++i) { im = i; for(k=i+1; k<=a.r-1; ++k) { if(lambda(k)>lambda(im)) im = k; } lm = lambda(i); lambda(i) = lambda(im); lambda(im) = lm; for(k=0; k<=a.r-1; ++k) { t(k) = x(k,i); x(k,i) = x(k,im); x(k,im) = t(k); } } return; } /* solve the generalized real eigenvalue problem a*x = lambda*b*x with positive b for all eigenvalues lambda and eigenvectors x (columns of x) */ void solvegeneralEVproblem(Dmatrix& a, Dmatrix& b, Dvector& lambda, Dmatrix& x) { int i; Dmatrix tb = b, ta, ty; i = tb.choldc(); if(i != 0) matrixerror("solvegeneralEVproblem - b not positive"); ta = tb.bwdsubst(a); ta.transpose(); ty = tb.bwdsubst(ta); Dmatrix t(b.r, b.r); solveEVproblem(ty, lambda, t); for(i=0; i<=b.r-1; ++i) ty.col(tb.bwdsubst_t(t.col(i)), i); x = ty; return; } /* ------------------------------------------------------------------- */ /* write pairs (x, y) to file name.xyf */ void toxyf(const char *name, Dvector x, Dvector y) { FILE *f; int l, i; char n[20]; l=0; while(l<=15 && name[l] != 0) {n[l] = name[l]; ++l;} n[l] = '.'; ++l; n[l] = 'x'; ++l; n[l] = 'y'; ++l; n[l] = 'f'; ++l; n[l] = 0; l = x.nel; if(y.nel < l) l = y.nel; fprintf(stderr, "(%d) >> %s\n", l, n); f = fopen(n, "w+"); for(i=0; i<=l-1; ++i) fprintf(f, "%.15g %.15g\n", x(i), y(i)); fclose(f); return; } /* write triples (x, y.re, y.im) to file name.xyf */ void toxyf(const char *name, Dvector x, Cvector y) { FILE *f; int l, i; char n[20]; l=0; while(l<=15 && name[l] != 0) {n[l] = name[l]; ++l;} n[l] = '.'; ++l; n[l] = 'x'; ++l; n[l] = 'y'; ++l; n[l] = 'f'; ++l; n[l] = 0; l = x.nel; if(y.nel < l) l = y.nel; fprintf(stderr, "(%d) >> %s\n", l, n); f = fopen(n, "w+"); for(i=0; i<=l-1; ++i) fprintf(f, "%.15g %.15g %.15g\n", x(i), y(i).re, y(i).im); fclose(f); return; } /* ------------------------------------------------------------------- */ /* sort a, b in parallel, such that a is ascending */ void sort(Dvector& a, Dvector &b) { if(a.nel != b.nel) matrixerror("sort(.,.): dimensions"); if(a.nel <= 1) return; int j, k, maxi; double maxel, t; for(j=0; j<=a.nel-2; ++j) { maxi = j; maxel = a.m[j]; for(k=j+1; k<=a.nel-1; ++k) { if(maxel>a.m[k]) { maxel = a.m[k]; maxi = k; } } if(j != maxi) { t = a.m[j]; a.m[j] = a.m[maxi]; a.m[maxi] = t; t = b.m[j]; b.m[j] = b.m[maxi]; b.m[maxi] = t; } } return; } /* ------------------------------------------------------------------- */ /* - third order arrays ---------------------------------------------- */ /* ------------------------------------------------------------------- */ /* Barray3: threedimensional bool array */ /* initialize */ Barray3::Barray3() : d1(0), d2(0), d3(0), d1d2(0), dim(0) { m = NULL; } Barray3::Barray3(int dim1, int dim2, int dim3) : d1(dim1), d2(dim2), d3(dim3), d1d2(dim1*dim2), dim(dim1*dim2*dim3) { m = new bool[dim]; #ifdef INITARR init(false); #endif } /* destroy */ Barray3::~Barray3() { if(m != NULL) delete[] m; m = NULL; d1 = d2 = d3 = d1d2 = dim = 0; } /* copy constructor */ Barray3::Barray3(const Barray3& s) : d1(s.d1), d2(s.d2), d3(s.d3), d1d2(s.d1d2), dim(s.dim) { m = new bool[dim]; bool* sp = s.m; bool* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = *sp++; } /* assignment */ Barray3& Barray3::operator=(const Barray3& s) { if(this != &s) { if(m != NULL) delete[] m; d1 = s.d1; d2 = s.d2; d3 = s.d3; d1d2 = s.d1d2; dim = s.dim; m = new bool[dim]; bool* sp = s.m; bool* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = *sp++; } return *this; } /* free allocated memory */ void Barray3::strip() { d1 = 0; d2 = 0; d3 = 0; d1d2 = 0; dim = 0; if(m != NULL) delete[] m; m = NULL; } /* set all elements to i */ void Barray3::init(bool b) { bool* mp = m; for(int j=0; j<=dim-1; ++j) *mp++ = b; return; } /* output to FILE dat */ void Barray3::write(FILE *dat) const { if(dat == stderr || dat == stdout) { fprintf(dat, "D1: %d, D2: %d, D3: %d, Dim: %d\n", d1, d2, d3, dim); for(int i=0; i<=d1-1; ++i) { for(int j=0; j<=d2-1; ++j) { for(int k=0; k<=d3-1; ++k) { fprintf(dat, "%d ", (int)(*this)(i,j,k)); } fprintf(dat, "\n"); } fprintf(dat, "\n\n"); } } else { comment("d1", dat); fputint(d1, dat); comment("d2", dat); fputint(d2, dat); comment("d3", dat); fputint(d3, dat); comment("[][][]", dat); for(int i=0; i<=dim-1; ++i) fputint((int)m[i], dat); } return; } /* input from FILE dat */ void Barray3::read(FILE *dat) { if(dat == stderr || dat == stdout) { matrixerror("Barray3: read"); } else { if(m != NULL) delete[] m; d1 = fgetint(dat); d2 = fgetint(dat); d3 = fgetint(dat); d1d2 = d1*d2; dim = d1d2*d3; m = new bool[dim]; for(int i=0; i<=dim-1; ++i) m[i] = (bool)fgetint(dat); } return; } /* ------------------------------------------------------------------- */ /* Iarray3: threedimensional integer array */ /* initialize */ Iarray3::Iarray3() : d1(0), d2(0), d3(0), d1d2(0), dim(0) { m = NULL; } Iarray3::Iarray3(int dim1, int dim2, int dim3) : d1(dim1), d2(dim2), d3(dim3), d1d2(dim1*dim2), dim(dim1*dim2*dim3) { m = new int[dim]; #ifdef INITARR init(0); #endif } /* destroy */ Iarray3::~Iarray3() { if(m != NULL) delete[] m; m = NULL; d1 = d2 = d3 = d1d2 = dim = 0; } /* copy constructor */ Iarray3::Iarray3(const Iarray3& s) : d1(s.d1), d2(s.d2), d3(s.d3), d1d2(s.d1d2), dim(s.dim) { m = new int[dim]; int* sp = s.m; int* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = *sp++; } /* assignment */ Iarray3& Iarray3::operator=(const Iarray3& s) { if(this != &s) { if(m != NULL) delete[] m; d1 = s.d1; d2 = s.d2; d3 = s.d3; d1d2 = s.d1d2; dim = s.dim; m = new int[dim]; int* sp = s.m; int* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = *sp++; } return *this; } /* free allocated memory */ void Iarray3::strip() { d1 = 0; d2 = 0; d3 = 0; d1d2 = 0; dim = 0; if(m != NULL) delete[] m; m = NULL; } /* set all elements to i */ void Iarray3::init(int b) { int* mp = m; for(int j=0; j<=dim-1; ++j) *mp++ = b; return; } /* output to FILE dat */ void Iarray3::write(FILE *dat) const { if(dat == stderr || dat == stdout) { fprintf(dat, "D1: %d, D2: %d, D3: %d, Dim: %d\n", d1, d2, d3, dim); for(int i=0; i<=d1-1; ++i) { for(int j=0; j<=d2-1; ++j) { for(int k=0; k<=d3-1; ++k) { fprintf(dat, "%d ", (*this)(i,j,k)); } fprintf(dat, "\n"); } fprintf(dat, "\n\n"); } } else { comment("d1", dat); fputint(d1, dat); comment("d2", dat); fputint(d2, dat); comment("d3", dat); fputint(d3, dat); comment("[][][]", dat); for(int i=0; i<=dim-1; ++i) fputint(m[i], dat); } return; } /* input from FILE dat */ void Iarray3::read(FILE *dat) { if(dat == stderr || dat == stdout) { matrixerror("Iarray3: read"); } else { if(m != NULL) delete[] m; d1 = fgetint(dat); d2 = fgetint(dat); d3 = fgetint(dat); d1d2 = d1*d2; dim = d1d2*d3; m = new int[dim]; for(int i=0; i<=dim-1; ++i) m[i] = fgetint(dat); } return; } /* ------------------------------------------------------------------- */ /* Darray3: threedimensional double array */ /* initialize */ Darray3::Darray3() : d1(0), d2(0), d3(0), d1d2(0), dim(0) { m = NULL; } Darray3::Darray3(int dim1, int dim2, int dim3) : d1(dim1), d2(dim2), d3(dim3), d1d2(dim1*dim2), dim(dim1*dim2*dim3) { m = new double[dim]; #ifdef INITARR init(0.0); #endif } /* destroy */ Darray3::~Darray3() { if(m != NULL) delete[] m; m = NULL; d1 = d2 = d3 = d1d2 = dim = 0; } /* copy constructor */ Darray3::Darray3(const Darray3& s) : d1(s.d1), d2(s.d2), d3(s.d3), d1d2(s.d1d2), dim(s.dim) { m = new double[dim]; double* sp = s.m; double* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = *sp++; } /* assignment */ Darray3& Darray3::operator=(const Darray3& s) { if(this != &s) { if(m != NULL) delete[] m; d1 = s.d1; d2 = s.d2; d3 = s.d3; d1d2 = s.d1d2; dim = s.dim; m = new double[dim]; double* sp = s.m; double* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = *sp++; } return *this; } /* free allocated memory */ void Darray3::strip() { d1 = 0; d2 = 0; d3 = 0; d1d2 = 0; dim = 0; if(m != NULL) delete[] m; m = NULL; } /* set all elements to i */ void Darray3::init(double b) { double* mp = m; for(int j=0; j<=dim-1; ++j) *mp++ = b; return; } /* output to FILE dat */ void Darray3::write(FILE *dat) const { if(dat == stderr || dat == stdout) { fprintf(dat, "D1: %d, D2: %d, D3: %d, Dim: %d\n", d1, d2, d3, dim); for(int i=0; i<=d1-1; ++i) { for(int j=0; j<=d2-1; ++j) { for(int k=0; k<=d3-1; ++k) { fprintf(dat, "%g ", (*this)(i,j,k)); } fprintf(dat, "\n"); } fprintf(dat, "\n\n"); } } else { comment("d1", dat); fputint(d1, dat); comment("d2", dat); fputint(d2, dat); comment("d3", dat); fputint(d3, dat); comment("[][][]", dat); for(int i=0; i<=dim-1; ++i) fputdouble(m[i], dat); } return; } /* input from FILE dat */ void Darray3::read(FILE *dat) { if(dat == stderr || dat == stdout) { matrixerror("Darray3: read"); } else { if(m != NULL) delete[] m; d1 = fgetint(dat); d2 = fgetint(dat); d3 = fgetint(dat); d1d2 = d1*d2; dim = d1d2*d3; m = new double[dim]; for(int i=0; i<=dim-1; ++i) m[i] = fgetdouble(dat); } return; } /* ------------------------------------------------------------------- */ /* Carray3: threedimensional Complex array */ /* initialize */ Carray3::Carray3() : d1(0), d2(0), d3(0), d1d2(0), dim(0) { m = NULL; } Carray3::Carray3(int dim1, int dim2, int dim3) : d1(dim1), d2(dim2), d3(dim3), d1d2(dim1*dim2), dim(dim1*dim2*dim3) { m = new Complex[dim]; #ifdef INITARR init(CC0); #endif } /* destroy */ Carray3::~Carray3() { if(m != NULL) delete[] m; m = NULL; d1 = d2 = d3 = d1d2 = dim = 0; } /* copy constructor */ Carray3::Carray3(const Carray3& s) : d1(s.d1), d2(s.d2), d3(s.d3), d1d2(s.d1d2), dim(s.dim) { m = new Complex[dim]; Complex* sp = s.m; Complex* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = *sp++; } /* assignment */ Carray3& Carray3::operator=(const Carray3& s) { if(this != &s) { if(m != NULL) delete[] m; d1 = s.d1; d2 = s.d2; d3 = s.d3; d1d2 = s.d1d2; dim = s.dim; m = new Complex[dim]; Complex* sp = s.m; Complex* mp = m; for(int i=0; i<=dim-1; ++i) *mp++ = *sp++; } return *this; } /* free allocated memory */ void Carray3::strip() { d1 = 0; d2 = 0; d3 = 0; d1d2 = 0; dim = 0; if(m != NULL) delete[] m; m = NULL; } /* set all elements to i */ void Carray3::init(Complex b) { Complex* mp = m; for(int j=0; j<=dim-1; ++j) *mp++ = b; return; } /* output to FILE dat */ void Carray3::write(FILE *dat) const { if(dat == stderr || dat == stdout) { fprintf(dat, "D1: %d, D2: %d, D3: %d, Dim: %d\n", d1, d2, d3, dim); for(int i=0; i<=d1-1; ++i) { for(int j=0; j<=d2-1; ++j) { for(int k=0; k<=d3-1; ++k) { ((*this)(i,j,k)).write(dat); } fprintf(dat, "\n"); } fprintf(dat, "\n\n"); } } else { comment("d1", dat); fputint(d1, dat); comment("d2", dat); fputint(d2, dat); comment("d3", dat); fputint(d3, dat); comment("[][][]", dat); for(int i=0; i<=dim-1; ++i) m[i].write(dat); } return; } /* input from FILE dat */ void Carray3::read(FILE *dat) { if(dat == stderr || dat == stdout) { matrixerror("Carray3: read"); } else { if(m != NULL) delete[] m; d1 = fgetint(dat); d2 = fgetint(dat); d3 = fgetint(dat); d1d2 = d1*d2; dim = d1d2*d3; m = new Complex[dim]; for(int i=0; i<=dim-1; ++i) m[i].read(dat); } return; } #undef INITARR