xue@11: /*
xue@11:     Harmonic sinusoidal modelling and tools
xue@11: 
xue@11:     C++ code package for harmonic sinusoidal modelling and relevant signal processing.
xue@11:     Centre for Digital Music, Queen Mary, University of London.
xue@11:     This file copyright 2011 Wen Xue.
xue@11: 
xue@11:     This program is free software; you can redistribute it and/or
xue@11:     modify it under the terms of the GNU General Public License as
xue@11:     published by the Free Software Foundation; either version 2 of the
xue@11:     License, or (at your option) any later version.
xue@11: */
xue@1: //---------------------------------------------------------------------------
xue@1: #include <math.h>
xue@1: #include <memory.h>
Chris@2: #include "matrix.h"
Chris@5: 
Chris@5: /** \file matrix.h */
Chris@5: 
xue@1: //---------------------------------------------------------------------------
Chris@5: 
Chris@5: /**
xue@1:   function BalanceSim: applies a similarity transformation to matrix a so that a is "balanced". This is
xue@1:   used by various eigenvalue evaluation routines.
xue@1: 
xue@1:   In: matrix A[n][n]
xue@1:   Out: balanced matrix a
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void BalanceSim(int n, double** A)
xue@1: {
xue@1:   if (n<2) return;
xue@1:   const int radix=2;
xue@1:   double sqrdx;
xue@1:   sqrdx=radix*radix;
xue@1:   bool finish=false;
xue@1:   while (!finish)
xue@1:   {
xue@1:     finish=true;
xue@1:     for (int i=0; i<n; i++)
xue@1:     {
xue@1:       double s, sr=0, sc=0, ar, ac;
xue@1:       for (int j=0; j<n; j++)
xue@1:         if (j!=i)
xue@1:         {
xue@1:           sc+=fabs(A[j][i]);
xue@1:           sr+=fabs(A[i][j]);
xue@1:         }
xue@1:       if (sc!=0 && sr!=0)
xue@1:       {
xue@1:         ar=sr/radix;
xue@1:         ac=1.0;
xue@1:         s=sr+sc;
xue@1:         while (sc<ar)
xue@1:         {
xue@1:           ac*=radix;
xue@1:           sc*=sqrdx;
xue@1:         }
xue@1:         ar=sr*radix;
xue@1:         while (sc>ar)
xue@1:         {
xue@1:           ac/=radix;
xue@1:           sc/=sqrdx;
xue@1:         }
xue@1:       }
xue@1:       if ((sc+sr)/ac<0.95*s)
xue@1:       {
xue@1:         finish=false;
xue@1:         ar=1.0/ac;
xue@1:         for (int j=0; j<n; j++) A[i][j]*=ar;
xue@1:         for (int j=0; j<n; j++) A[j][i]*=ac;
xue@1:       }
xue@1:     }
xue@1:   }
xue@1: }//BalanceSim
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function Choleski: Choleski factorization A=LL', where L is lower triangular. The symmetric matrix
xue@1:   A[N][N] is positive definite iff A can be factored as LL', where L is lower triangular with nonzero
xue@1:   diagonl entries.
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: mstrix L[N][N].
xue@1: 
xue@1:   Returns 0 if successful. On return content of matrix a is not changed.
xue@1: */
xue@1: int Choleski(int N, double** L, double** A)
xue@1: {
xue@1:   if (A[0][0]==0) return 1;
xue@1:   L[0][0]=sqrt(A[0][0]);
xue@1:   memset(&L[0][1], 0, sizeof(double)*(N-1));
xue@1:   for (int j=1; j<N; j++) L[j][0]=A[j][0]/L[0][0];
xue@1:   for (int i=1; i<N-1; i++)
xue@1:   {
xue@1:     L[i][i]=A[i][i]; for (int k=0; k<i; k++) L[i][i]-=L[i][k]*L[i][k]; L[i][i]=sqrt(L[i][i]);
xue@1:     if (L[i][i]==0) return 1;
xue@1:     for (int j=i+1; j<N; j++)
xue@1:     {
xue@1:       L[j][i]=A[j][i]; for (int k=0; k<i; k++) L[j][i]-=L[j][k]*L[i][k]; L[j][i]/=L[i][i];
xue@1:     }
xue@1:     memset(&L[i][i+1], 0, sizeof(double)*(N-1-i));
xue@1:   }
xue@1:   L[N-1][N-1]=A[N-1][N-1]; for (int k=0; k<N-1; k++) L[N-1][N-1]-=L[N-1][k]*L[N-1][k]; L[N-1][N-1]=sqrt(L[N-1][N-1]);
xue@1:   return 0;
xue@1: }//Choleski
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: //matrix duplication routines
xue@1: 
Chris@5: /**
xue@1:   function Copy: duplicate the matrix A as matrix Z.
xue@1: 
xue@1:   In: matrix A[M][N]
xue@1:   Out: matrix Z[M][N]
xue@1: 
xue@1:   Returns pointer to Z. Z is created anew if Z=0 is supplied on start.
xue@1: */
xue@1: double** Copy(int M, int N, double** Z, double** A, MList* List)
xue@1: {
xue@1:   if (!Z) {Allocate2(double, M, N, Z); if (List) List->Add(Z, 2);}
xue@1:   int sizeN=sizeof(double)*N;
xue@1:   for (int m=0; m<M; m++) memcpy(Z[m], A[m], sizeN);
xue@1:   return Z;
xue@1: }//Copy
xue@1: //complex version
xue@1: cdouble** Copy(int M, int N, cdouble** Z, cdouble** A, MList* List)
xue@1: {
xue@1:   if (!Z) {Allocate2(cdouble, M, N, Z); if (List) List->Add(Z, 2);}
xue@1:   int sizeN=sizeof(cdouble)*N;
xue@1:   for (int m=0; m<M; m++) memcpy(Z[m], A[m], sizeN);
xue@1:   return Z;
xue@1: }//Copy
xue@1: //version without specifying pre-allocated z
xue@1: double** Copy(int M, int N, double** A, MList* List){return Copy(M, N, 0, A, List);}
xue@1: cdouble** Copy(int M, int N, cdouble** A, MList* List){return Copy(M, N, 0, A, List);}
xue@1: //for square matrices
xue@1: double** Copy(int N, double** Z, double ** A, MList* List){return Copy(N, N, Z, A, List);}
xue@1: double** Copy(int N, double** A, MList* List){return Copy(N, N, 0, A, List);}
xue@1: cdouble** Copy(int N, cdouble** Z, cdouble** A, MList* List){return Copy(N, N, Z, A, List);}
xue@1: cdouble** Copy(int N, cdouble** A, MList* List){return Copy(N, N, 0, A, List);}
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: //vector duplication routines
xue@1: 
Chris@5: /**
xue@1:   function Copy: duplicating vector a as vector z
xue@1: 
xue@1:   In: vector a[N]
xue@1:   Out: vector z[N]
xue@1: 
xue@1:   Returns pointer to z. z is created anew is z=0 is specified on start.
xue@1: */
xue@1: double* Copy(int N, double* z, double* a, MList* List)
xue@1: {
xue@1:   if (!z){z=new double[N]; if (List) List->Add(z, 1);}
xue@1:   memcpy(z, a, sizeof(double)*N);
xue@1:   return z;
xue@1: }//Copy
xue@1: cdouble* Copy(int N, cdouble* z, cdouble* a, MList* List)
xue@1: {
xue@1:   if (!z){z=new cdouble[N]; if (List) List->Add(z, 1);}
xue@1:   memcpy(z, a, sizeof(cdouble)*N);
xue@1:   return z;
xue@1: }//Copy
xue@1: //version without specifying pre-allocated z
xue@1: double* Copy(int N, double* a, MList* List){return Copy(N, 0, a, List);}
xue@1: cdouble* Copy(int N, cdouble* a, MList* List){return Copy(N, 0, a, List);}
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function det: computes determinant by Gaussian elimination method with column pivoting
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1: 
xue@1:   Returns det(A). On return content of matrix A is unchanged if mode=0.
xue@1: */
xue@1: double det(int N, double** A, int mode)
xue@1: {
xue@1:   int c, p, ip, *rp=new int[N]; for (int i=0; i<N; i++) rp[i]=i;
xue@1:   double m, **b, result=1;
xue@1: 
xue@1:   if (mode==0)
xue@1:   {
xue@1:     int sizeN=sizeof(double)*N;
xue@1:     b=new double*[N]; b[0]=new double[N*N]; for (int i=0; i<N; i++) {b[i]=&b[0][i*N]; memcpy(b[i], A[i], sizeN);}
xue@1:     A=b;
xue@1:   }
xue@1: 
xue@1:   //Gaussian eliminating
xue@1:   for (int i=0; i<N-1; i++)
xue@1:   {
xue@1:     p=i, ip=i+1;
xue@1:     while (ip<N){if (fabs(A[rp[ip]][i])>fabs(A[rp[p]][i])) p=ip; ip++;}
xue@1:     if (A[rp[p]][i]==0) {result=0; goto ret;}
xue@1:     if (p!=i) {c=rp[i]; rp[i]=rp[p]; rp[p]=c; result=-result;}
xue@1:     for (int j=i+1; j<N; j++)
xue@1:     {
xue@1:       m=A[rp[j]][i]/A[rp[i]][i];
xue@1:       A[rp[j]][i]=0;
xue@1:       for (int k=i+1; k<N; k++) A[rp[j]][k]-=m*A[rp[i]][k];
xue@1:     }
xue@1:   }
xue@1:   if (A[rp[N-1]][N-1]==0) {result=0; goto ret;}
xue@1: 
xue@1: 	for (int i=0; i<N; i++)
xue@1:     result*=A[rp[i]][i];
xue@1: 
xue@1: ret:
xue@1:   if (mode==0) {delete[] b[0]; delete[] b;}
xue@1:   delete[] rp;
xue@1:   return result;
xue@1: }//det
xue@1: //complex version
xue@1: cdouble det(int N, cdouble** A, int mode)
xue@1: {
xue@1:   int c, p, ip, *rp=new int[N]; for (int i=0; i<N; i++) rp[i]=i;
xue@1:   double mm, mp;
xue@1:   cdouble m, **b, result=1;
xue@1: 
xue@1:   if (mode==0)
xue@1:   {
xue@1:     int sizeN=sizeof(cdouble)*N;
xue@1:     b=new cdouble*[N]; b[0]=new cdouble[N*N];
xue@1:     for (int i=0; i<N; i++) {b[i]=&b[0][i*N]; memcpy(b[i], A[i], sizeN);}
xue@1:     A=b;
xue@1:   }
xue@1: 
xue@1:   //Gaussian elimination
xue@1:   for (int i=0; i<N-1; i++)
xue@1:   {
xue@1:     p=i, ip=i+1; m=A[rp[p]][i]; mp=~m;
xue@1:     while (ip<N){m=A[rp[ip]][i]; mm=~m; if (mm>mp) mp=mm, p=ip; ip++;}
xue@1:     if (mp==0) {result=0; goto ret;}
xue@1:     if (p!=i) {c=rp[i]; rp[i]=rp[p]; rp[p]=c;}
xue@1:     for (int j=i+1; j<N; j++)
xue@1:     {
xue@1:       m=A[rp[j]][i]/A[rp[i]][i];
xue@1:       A[rp[j]][i]=0;
xue@1:       for (int k=i+1; k<N; k++) A[rp[j]][k]-=m*A[rp[i]][k];
xue@1:     }
xue@1:   }
xue@1:   if (operator==(A[rp[N-1]][N-1],0)) {result=0; goto ret;}
xue@1: 
xue@1:   for (int i=0; i<N; i++) result=result*A[rp[i]][i];
xue@1: ret:
xue@1:   if (mode==0) {delete[] b[0]; delete[] b;}
xue@1:   delete[] rp;
xue@1:   return result;
xue@1: }//det
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function EigPower: power method for solving dominant eigenvalue and eigenvector
xue@1: 
xue@1:   In: matrix A[N][N], initial arbitrary vector x[N].
xue@1:   Out: eigenvalue l, eigenvector x[N].
xue@1: 
xue@1:   Returns 0 is successful. Content of matrix A is unchangd on return. Initial x[N] must not be zero.
xue@1: */
xue@1: int EigPower(int N, double& l, double* x, double** A, double ep, int maxiter)
xue@1: {
xue@1:   int k=0;
xue@1:   int p=0; for (int i=1; i<N; i++) if (fabs(x[p])<fabs(x[i])) p=i;
xue@1:   Multiply(N, x, x, 1/x[p]);
xue@1:   double e, ty,te, *y=new double[N];
xue@1: 
xue@1:   while (k<maxiter)
xue@1:   {
xue@1:     MultiplyXy(N, N, y, A, x);
xue@1:     l=y[p];
xue@1:     int p=0; for (int i=1; i<N; i++) if (fabs(y[p])<fabs(y[i])) p=i;
xue@1:     if (y[p]==0) {l=0; delete[] y; return 0;}
xue@1:     ty=y[0]/y[p]; e=fabs(x[0]-ty); x[0]=ty;
xue@1:     for (int i=1; i<N; i++)
xue@1:     {
xue@1:       ty=y[i]/y[p]; te=fabs(x[i]-ty); if (e<te) e=te; x[i]=ty;
xue@1:     }
xue@1:     if (e<ep) {delete[] y; return 0;}
xue@1:     k++;
xue@1:   }
xue@1:   delete[] y; return 1;
xue@1: }//EigPower
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function EigPowerA: EigPower with Aitken acceleration
xue@1: 
xue@1:   In: matrix A[N][N], initial arbitrary vector x[N].
xue@1:   Out: eigenvalue l, eigenvector x[N].
xue@1: 
xue@1:   Returns 0 is successful. Content of matrix A is unchangd on return. Initial x[N] must not be zero.
xue@1: */
xue@1: int EigPowerA(int N, double& l, double* x, double** A, double ep, int maxiter)
xue@1: {
xue@1:   int k=0;
xue@1:   int p=0; for (int i=1; i<N; i++) if (fabs(x[p])<fabs(x[i])) p=i;
xue@1:   Multiply(N, x, x, 1/x[p]);
xue@1:   double m, m0=0, m1=0, e, ty,te, *y=new double[N];
xue@1: 
xue@1:   while (k<maxiter)
xue@1:   {
xue@1:     MultiplyXy(N, N, y, A, x);
xue@1:     m=y[p];
xue@1:     int p=0; for (int i=1; i<N; i++) if (fabs(y[p])<fabs(y[i])) p=i;
xue@1:     if (y[p]==0) {l=0; delete[] y; return 0;}
xue@1:     ty=y[0]/y[p]; e=fabs(x[0]-ty); x[0]=ty;
xue@1:     for (int i=1; i<N; i++)
xue@1:     {
xue@1:       ty=y[i]/y[p]; te=fabs(x[i]-ty); if (e<te) e=te; x[i]=ty;
xue@1:     }
xue@1:     if (e<ep && k>2) {l=m0-(m1-m0)*(m1-m0)/(m-2*m1+m0); delete[] y; return 0;}
xue@1:     k++; m0=m1; m1=m;
xue@1:   }
xue@1:   delete[] y; return 1;
xue@1: }//EigPowerA
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function EigPowerI: Inverse power method for solving the eigenvalue given an approximate non-zero
xue@1:   eigenvector.
xue@1: 
xue@1:   In: matrix A[N][N], approximate eigenvector x[N].
xue@1:   Out: eigenvalue l, eigenvector x[N].
xue@1: 
xue@1:   Returns 0 is successful. Content of matrix A is unchangd on return. Initial x[N] must not be zero.
xue@1: */
xue@1: int EigPowerI(int N, double& l, double* x, double** A, double ep, int maxiter)
xue@1: {
xue@1:   int sizeN=sizeof(double)*N;
xue@1:   double* y=new double[N]; MultiplyXy(N, N, y, A, x);
xue@1:   double q=Inner(N, x, y)/Inner(N, x, x), dt;
xue@1:   double** aa=new double*[N]; aa[0]=new double[N*N];
xue@1:   for (int i=0; i<N; i++) {aa[i]=&aa[0][i*N]; memcpy(aa[i], A[i], sizeN); aa[i][i]-=q;}
xue@1:   dt=GISCP(N, aa);
xue@1:   if (dt==0) {l=q; delete[] aa[0]; delete[] aa; delete[] y; return 0;}
xue@1: 
xue@1:   int k=0;
xue@1:   int p=0; for (int i=1; i<N; i++) if (fabs(x[p])<fabs(x[i])) p=i;
xue@1:   Multiply(N, x, x, 1/x[p]);
xue@1: 
xue@1:   double m, e, ty, te;
xue@1:   while (k<N)
xue@1:   {
xue@1:     MultiplyXy(N, N, y, aa, x);
xue@1:     m=y[p];
xue@1:     p=0; for (int i=1; i<N; i++) if (fabs(y[p])<fabs(y[i])) p=i;
xue@1:     ty=y[0]/y[p]; te=x[0]-ty; e=fabs(te); x[0]=ty;
xue@1:     for (int i=1; i<N; i++)
xue@1:     {
xue@1:       ty=y[i]/y[p]; te=fabs(x[i]-ty); if (e<te) e=te; x[i]=ty;
xue@1:     }
xue@1:     if (e<ep) {l=1/m+q; delete[] aa[0]; delete[] aa; delete[] y; return 0;}
xue@1:   }
xue@1:   delete[] aa[0]; delete[] aa;
xue@1:   delete[] y; return 1;
xue@1: }//EigPowerI
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function EigPowerS: symmetric power method for solving the dominant eigenvalue with its eigenvector
xue@1: 
xue@1:   In: matrix A[N][N], initial arbitrary vector x[N].
xue@1:   Out: eigenvalue l, eigenvector x[N].
xue@1: 
xue@1:   Returns 0 is successful. Content of matrix A is unchangd on return. Initial x[N] must not be zero.
xue@1: */
xue@1: int EigPowerS(int N, double& l, double* x, double** A, double ep, int maxiter)
xue@1: {
xue@1:   int k=0;
xue@1:   Multiply(N, x, x, 1/sqrt(Inner(N, x, x)));
xue@1:   double y2, e, ty, te, *y=new double[N];
xue@1:   while (k<maxiter)
xue@1:   {
xue@1:     MultiplyXy(N, N, y, A, x);
xue@1:     l=Inner(N, x, y);
xue@1:     y2=sqrt(Inner(N, y, y));
xue@1:     if (y2==0) {l=0; delete[] y; return 0;}
xue@1:     ty=y[0]/y2; te=x[0]-ty; e=te*te; x[0]=ty;
xue@1:     for (int i=1; i<N; i++)
xue@1:     {
xue@1:       ty=y[i]/y2; te=x[i]-ty; e+=te*te; x[i]=ty;
xue@1:     }
xue@1:     e=sqrt(e);
xue@1:     if (e<ep) {delete[] y; return 0;}
xue@1:     k++;
xue@1:   }
xue@1:   delete[] y;
xue@1:   return 1;
xue@1: }//EigPowerS
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function EigPowerWielandt: Wielandt's deflation algorithm for solving a second dominant eigenvalue and
xue@1:   eigenvector (m,u) given the dominant eigenvalue and eigenvector (l,v).
xue@1: 
xue@1:   In: matrix A[N][N], first eigenvalue l with eigenvector v[N]
xue@1:   Out: second eigenvalue m with eigenvector u
xue@1: 
xue@1:   Returns 0 if successful. Content of matrix A is unchangd on return. Initial u[N] must not be zero.
xue@1: */
xue@1: int EigPowerWielandt(int N, double& m, double* u, double l, double* v, double** A, double ep, int maxiter)
xue@1: {
xue@1:   int result;
xue@1:   double** b=new double*[N-1]; b[0]=new double[(N-1)*(N-1)]; for (int i=1; i<N-1; i++) b[i]=&b[0][i*(N-1)];
xue@1:   double* w=new double[N];
xue@1:   int i=0; for (int j=1; j<N; j++) if (fabs(v[i])<fabs(v[j])) i=j;
xue@1:   if (i!=0)
xue@1:     for (int k=0; k<i; k++)
xue@1:       for (int j=0; j<i; j++)
xue@1:         b[k][j]=A[k][j]-v[k]*A[i][j]/v[i];
xue@1:   if (i!=0 && i!=N-1)
xue@1:     for (int k=i; k<N-1; k++)
xue@1:       for (int j=0; j<i; j++)
xue@1:         b[k][j]=A[k+1][j]-v[k+1]*A[i][j]/v[i], b[j][k]=A[j][k+1]-v[j]*A[i][k+1]/v[i];
xue@1:   if (i!=N-1)
xue@1:     for (int k=i; k<N-1; k++)
xue@1:       for (int j=i; j<N-1; j++) b[k][j]=A[k+1][j+1]-v[k+1]*A[i][j+1]/v[i];
xue@1:   memcpy(w, u, sizeof(double)*(N-1));
xue@1:   if ((result=EigPower(N-1, m, w, b, ep, maxiter))==0)
xue@1:   { //*
xue@1:     if (i!=N-1) memmove(&w[i+1], &w[i], sizeof(double)*(N-i-1));
xue@1:     w[i]=0;
xue@1:     for (int k=0; k<N; k++) u[k]=(m-l)*w[k]+Inner(N, A[i], w)*v[k]/v[i];   //*/
xue@1:   }
xue@1:   delete[] w; delete[] b[0]; delete[] b;
xue@1:   return result;
xue@1: }//EigPowerWielandt
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: //NR versions of eigensystem
xue@1: 
Chris@5: /**
xue@1:   function EigenValues: solves for eigenvalues of general system
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: eigenvalues ev[N]
xue@1: 
xue@1:   Returns 0 if successful. Content of matrix A is destroyed on return.
xue@1: */
xue@1: int EigenValues(int N, double** A, cdouble* ev)
xue@1: {
xue@1:   BalanceSim(N, A);
xue@1:   Hessenb(N, A);
xue@1:   return QR(N, A, ev);
xue@1: }//EigenValues
xue@1: 
Chris@5: /**
xue@1:   function EigSym: Solves real symmetric eigensystem A
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: eigenvalues d[N], transform matrix Q[N][N], so that diag(d)=Q'AQ, A=Q diag(d) Q', AQ=Q diag(d)
xue@1: 
xue@1:   Returns 0 if successful. Content of matrix A is unchanged on return.
xue@1: */
xue@1: int EigSym(int N, double** A, double* d, double** Q)
xue@1: {
xue@1:   Copy(N, Q, A);
xue@1:   double* t=new double[N];
xue@1:   HouseHolder(5, Q, d, t);
xue@1:   double result=QL(5, d, t, Q);
xue@1:   delete[] t;
xue@1:   return result;
xue@1: }//EigSym
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function GEB: Gaussian elimination with backward substitution for solving linear system Ax=b.
xue@1: 
xue@1:   In: coefficient matrix A[N][N], vector b[N]
xue@1:   Out: vector x[N]
xue@1: 
xue@1:   Returns 0 if successful. Contents of matrix A and vector b are destroyed on return.
xue@1: */
xue@1: int GEB(int N, double* x, double** A, double* b)
xue@1: {
xue@1:   //Gaussian eliminating
xue@1:   int c, p, *rp=new int[N]; for (int i=0; i<N; i++) rp[i]=i;
xue@1:   double m;
xue@1:   for (int i=0; i<N-1; i++)
xue@1:   {
xue@1:     p=i;
xue@1:     while (p<N && A[rp[p]][i]==0) p++;
xue@1:     if (p>=N) {delete[] rp; return 1;}
xue@1:     if (p!=i){c=rp[i]; rp[i]=rp[p]; rp[p]=c;}
xue@1:     for (int j=i+1; j<N; j++)
xue@1:     {
xue@1:       m=A[rp[j]][i]/A[rp[i]][i];
xue@1:       A[rp[j]][i]=0;
xue@1:       for (int k=i+1; k<N; k++) A[rp[j]][k]-=m*A[rp[i]][k];
xue@1:       b[rp[j]]-=m*b[rp[i]];
xue@1:     }
xue@1:   }
xue@1:   if (A[rp[N-1]][N-1]==0) {delete[] rp; return 1;}
xue@1:   else
xue@1:   {
xue@1:     //backward substitution
xue@1:     x[N-1]=b[rp[N-1]]/A[rp[N-1]][N-1];
xue@1:     for (int i=N-2; i>=0; i--)
xue@1:     {
xue@1:       x[i]=b[rp[i]]; for (int j=i+1; j<N; j++) x[i]-=A[rp[i]][j]*x[j]; x[i]/=A[rp[i]][i];
xue@1:     }
xue@1:   }
xue@1:   delete[] rp;
xue@1:   return 0;
xue@1: }//GEB
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function GESCP: Gaussian elimination with scaled column pivoting for solving linear system Ax=b
xue@1: 
xue@1:   In: matrix A[N][N], vector b[N]
xue@1:   Out: vector x[N]
xue@1: 
xue@1:   Returns 0 is successful. Contents of matrix A and vector b are destroyed on return.
xue@1: */
xue@1: int GESCP(int N, double* x, double** A, double *b)
xue@1: {
xue@1:   int c, p, ip, *rp=new int[N];
xue@1:   double m, *s=new double[N];
xue@1:   for (int i=0; i<N; i++)
xue@1:   {
xue@1:     s[i]=fabs(A[i][0]);
xue@1:     for (int j=1; j<N; j++) if (s[i]<fabs(A[i][j])) s[i]=fabs(A[i][j]);
xue@1:     if (s[i]==0) {delete[] s; delete[] rp; return 1;}
xue@1:     rp[i]=i;
xue@1:   }
xue@1:   //Gaussian eliminating
xue@1:   for (int i=0; i<N-1; i++)
xue@1:   {
xue@1:     p=i, ip=i+1;
xue@1:     while (ip<N){if (fabs(A[rp[ip]][i])/s[rp[ip]]>fabs(A[rp[p]][i])/s[rp[p]]) p=ip; ip++;}
xue@1:     if (A[rp[p]][i]==0) {delete[] s; delete[] rp; return 1;}
xue@1:     if (p!=i) {c=rp[i]; rp[i]=rp[p]; rp[p]=c;}
xue@1:     for (int j=i+1; j<N; j++)
xue@1:     {
xue@1:       m=A[rp[j]][i]/A[rp[i]][i];
xue@1:       A[rp[j]][i]=0;
xue@1:       for (int k=i+1; k<N; k++) A[rp[j]][k]-=m*A[rp[i]][k];
xue@1:       b[rp[j]]-=m*b[rp[i]];
xue@1:     }
xue@1:   }
xue@1:   if (A[rp[N-1]][N-1]==0) {delete[] s; delete[] rp; return 1;}
xue@1:   //backward substitution
xue@1:   x[N-1]=b[rp[N-1]]/A[rp[N-1]][N-1];
xue@1:   for (int i=N-2; i>=0; i--)
xue@1:   {
xue@1:     x[i]=b[rp[i]]; for (int j=i+1; j<N; j++) x[i]-=A[rp[i]][j]*x[j]; x[i]/=A[rp[i]][i];
xue@1:   }
xue@1:   delete[] s; delete[] rp;
xue@1:   return 0;
xue@1: }//GESCP
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function GExL: solves linear system xL=a, L being lower-triangular. This is used in LU factorization
xue@1:   for solving linear systems.
xue@1: 
xue@1:   In: lower-triangular matrix L[N][N], vector a[N]
xue@1:   Out: vector x[N]
xue@1: 
xue@1:   No return value. Contents of matrix L and vector a are unchanged at return.
xue@1: */
xue@1: void GExL(int N, double* x, double** L, double* a)
xue@1: {
xue@1:   for (int n=N-1; n>=0; n--)
xue@1:   {
xue@1:     double xn=a[n];
xue@1:     for (int m=n+1; m<N; m++) xn-=x[m]*L[m][n];
xue@1:     x[n]=xn/L[n][n];
xue@1:   }
xue@1: }//GExL
xue@1: 
Chris@5: /**
xue@1:   function GExLAdd: solves linear system *L=a, L being lower-triangular, and add the solution * to x[].
xue@1: 
xue@1:   In: lower-triangular matrix L[N][N], vector a[N]
xue@1:   Out: updated vector x[N]
xue@1: 
xue@1:   No return value. Contents of matrix L and vector a are unchanged at return.
xue@1: */
xue@1: void GExLAdd(int N, double* x, double** L, double* a)
xue@1: {
xue@1:   double* lx=new double[N];
xue@1:   GExL(N, lx, L, a);
xue@1:   for (int i=0; i<N; i++) x[i]+=lx[i];
xue@1:   delete[] lx;
xue@1: }//GExLAdd
xue@1: 
Chris@5: /**
xue@1:   function GExL1: solves linear system xL=(0, 0, ..., 0, a)', L being lower-triangular.
xue@1: 
xue@1:   In: lower-triangular matrix L[N][N], a
xue@1:   Out: vector x[N]
xue@1: 
xue@1:   No return value. Contents of matrix L and vector a are unchanged at return.
xue@1: */
xue@1: void GExL1(int N, double* x, double** L, double a)
xue@1: {
xue@1:   double xn=a;
xue@1:   for (int n=N-1; n>=0; n--)
xue@1:   {
xue@1:     for (int m=n+1; m<N; m++) xn-=x[m]*L[m][n];
xue@1:     x[n]=xn/L[n][n];
xue@1:     xn=0;
xue@1:   }
xue@1: }//GExL1
xue@1: 
Chris@5: /**
xue@1:   function GExL1Add: solves linear system *L=(0, 0, ..., 0, a)', L being lower-triangular, and add the
xue@1:   solution * to x[].
xue@1: 
xue@1:   In: lower-triangular matrix L[N][N], vector a
xue@1:   Out: updated vector x[N]
xue@1: 
xue@1:   No return value. Contents of matrix L and vector a are unchanged at return.
xue@1: */
xue@1: void GExL1Add(int N, double* x, double** L, double a)
xue@1: {
xue@1:   double* lx=new double[N];
xue@1:   GExL1(N, lx, L, a);
xue@1:   for (int i=0; i<N; i++) x[i]+=lx[i];
xue@1:   delete[] lx;
xue@1: }//GExL1Add
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function GICP: matrix inverse using Gaussian elimination with column pivoting: inv(A)->A.
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: matrix A[N][N]
xue@1: 
xue@1:   Returns the determinant of the inverse matrix, 0 on failure.
xue@1: */
xue@1: double GICP(int N, double** A)
xue@1: {
xue@1:   int c, p, ip, *rp=new int[N]; for (int i=0; i<N; i++) rp[i]=i;
xue@1:   double m, result=1;
xue@1: 
xue@1:   //Gaussian eliminating
xue@1:   for (int i=0; i<N-1; i++)
xue@1:   {
xue@1:     p=i, ip=i+1;
xue@1:     while (ip<N){if (fabs(A[rp[ip]][i])>fabs(A[rp[p]][i])) p=ip; ip++;}
xue@1:     if (A[rp[p]][i]==0) {delete[] rp; return 0;}
xue@1:     if (p!=i) {c=rp[i]; rp[i]=rp[p]; rp[p]=c; result=-result;}
xue@1:     result/=A[rp[i]][i];
xue@1:     for (int j=i+1; j<N; j++)
xue@1:     {
xue@1:       m=A[rp[j]][i]/A[rp[i]][i];
xue@1:       A[rp[j]][i]=-m;
xue@1:       for (int k=i+1; k<N; k++) A[rp[j]][k]-=m*A[rp[i]][k];
xue@1:       for (int k=0; k<i; k++) A[rp[j]][k]-=m*A[rp[i]][k];
xue@1:     }
xue@1:   }
xue@1:   if (A[rp[N-1]][N-1]==0) {delete[] rp; return 0;}
xue@1:   result/=A[rp[N-1]][N-1];
xue@1:   //backward substitution
xue@1:   for (int i=0; i<N-1; i++)
xue@1:   {
xue@1:     m=A[rp[i]][i]; for (int k=0; k<N; k++) A[rp[i]][k]/=m; A[rp[i]][i]=1/m;
xue@1:     for (int j=i+1; j<N; j++)
xue@1:     {
xue@1:       m=A[rp[i]][j]/A[rp[j]][j]; for (int k=0; k<N; k++) A[rp[i]][k]-=A[rp[j]][k]*m; A[rp[i]][j]=-m;
xue@1:     }
xue@1:   }
xue@1:   m=A[rp[N-1]][N-1]; for (int k=0; k<N-1; k++) A[rp[N-1]][k]/=m; A[rp[N-1]][N-1]=1/m;
xue@1:   //recover column and row exchange
xue@1:   double* tm=new double[N]; int sizeN=sizeof(double)*N;
xue@1:   for (int i=0; i<N; i++) { for (int j=0; j<N; j++) tm[rp[j]]=A[i][j]; memcpy(A[i], tm, sizeN); }
xue@1:   for (int j=0; j<N; j++) { for (int i=0; i<N; i++) tm[i]=A[rp[i]][j]; for (int i=0; i<N; i++) A[i][j]=tm[i];}
xue@1: 
xue@1:   delete[] tm; delete[] rp;
xue@1:   return result;
xue@1: }//GICP
xue@1: //complex version
xue@1: cdouble GICP(int N, cdouble** A)
xue@1: {
xue@1:   int c, p, ip, *rp=new int[N]; for (int i=0; i<N; i++) rp[i]=i;
xue@1:   cdouble m, result=1;
xue@1: 
xue@1:   //Gaussian eliminating
xue@1:   for (int i=0; i<N-1; i++)
xue@1:   {
xue@1:     p=i, ip=i+1;
xue@1:     while (ip<N){if (~A[rp[ip]][i]>~A[rp[p]][i]) p=ip; ip++;}
xue@1:     if (A[rp[p]][i]==0) {delete[] rp; return 0;}
xue@1:     if (p!=i) {c=rp[i]; rp[i]=rp[p]; rp[p]=c; result=-result;}
xue@1:     result=result/(A[rp[i]][i]);
xue@1:     for (int j=i+1; j<N; j++)
xue@1:     {
xue@1:       m=A[rp[j]][i]/A[rp[i]][i];
xue@1:       A[rp[j]][i]=-m;
xue@1:       for (int k=i+1; k<N; k++) A[rp[j]][k]-=m*A[rp[i]][k];
xue@1:       for (int k=0; k<i; k++) A[rp[j]][k]-=m*A[rp[i]][k];
xue@1:     }
xue@1:   }
xue@1:   if (A[rp[N-1]][N-1]==0) {delete[] rp; return 0;}
xue@1:   result=result/A[rp[N-1]][N-1];
xue@1:   //backward substitution
xue@1:   for (int i=0; i<N-1; i++)
xue@1:   {
xue@1:     m=A[rp[i]][i]; for (int k=0; k<N; k++) A[rp[i]][k]=A[rp[i]][k]/m; A[rp[i]][i]=cdouble(1)/m;
xue@1:     for (int j=i+1; j<N; j++)
xue@1:     {
xue@1:       m=A[rp[i]][j]/A[rp[j]][j]; for (int k=0; k<N; k++) A[rp[i]][k]-=A[rp[j]][k]*m; A[rp[i]][j]=-m;
xue@1:     }
xue@1:   }
xue@1:   m=A[rp[N-1]][N-1]; for (int k=0; k<N-1; k++) A[rp[N-1]][k]=A[rp[N-1]][k]/m; A[rp[N-1]][N-1]=cdouble(1)/m;
xue@1:   //recover column and row exchange
xue@1:   cdouble* tm=new cdouble[N]; int sizeN=sizeof(cdouble)*N;
xue@1:   for (int i=0; i<N; i++) { for (int j=0; j<N; j++) tm[rp[j]]=A[i][j]; memcpy(A[i], tm, sizeN); }
xue@1:   for (int j=0; j<N; j++) { for (int i=0; i<N; i++) tm[i]=A[rp[i]][j]; for (int i=0; i<N; i++) A[i][j]=tm[i];}
xue@1: 
xue@1:   delete[] tm; delete[] rp;
xue@1:   return result;
xue@1: }//GICP
xue@1: 
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function GILT: inv(lower trangular of A)->lower trangular of A
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: matrix A[N][N]
xue@1: 
xue@1:   Returns the determinant of the lower trangular of A
xue@1: */
xue@1: double GILT(int N, double** A)
xue@1: {
xue@1:   double result=1;
xue@1:   A[0][0]=1/A[0][0];
xue@1:   for (int i=1; i<N; i++)
xue@1:   {
xue@1:     result*=A[i][i];
xue@1:     double tmp=1/A[i][i];
xue@1:     for (int k=0; k<i; k++) A[i][k]*=tmp; A[i][i]=tmp;
xue@1:     for (int j=0; j<i; j++)
xue@1:     {
xue@1:       double tmp2=A[i][j];
xue@1:       for (int k=0; k<j; k++) A[i][k]-=A[j][k]*tmp2; A[i][j]=-A[j][j]*tmp2;
xue@1:     }
xue@1:   }
xue@1:   return result;
xue@1: }//GILT
xue@1: 
Chris@5: /**
xue@1:   function GIUT: inv(upper trangular of A)->upper trangular of A
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: matrix A[N][N]
xue@1: 
xue@1:   Returns the determinant of the upper trangular of A
xue@1: */
xue@1: double GIUT(int N, double** A)
xue@1: {
xue@1:   double result=1;
xue@1:   A[0][0]=1/A[0][0];
xue@1:   for (int i=1; i<N; i++)
xue@1:   {
xue@1:     result*=A[i][i];
xue@1:     double tmp=1/A[i][i];
xue@1:     for (int k=0; k<i; k++) A[k][i]*=tmp; A[i][i]=tmp;
xue@1:     for (int j=0; j<i; j++)
xue@1:     {
xue@1:       double tmp2=A[j][i];
xue@1:       for (int k=0; k<j; k++) A[k][i]-=A[k][j]*tmp2; A[j][i]=-A[j][j]*tmp2;
xue@1:     }
xue@1:   }
xue@1:   return result;
xue@1: }//GIUT
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function GISCP: matrix inverse using Gaussian elimination w. scaled column pivoting: inv(A)->A.
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: matrix A[N][N]
xue@1: 
xue@1:   Returns the determinant of the inverse matrix, 0 on failure.
xue@1: */
xue@1: double GISCP(int N, double** A)
xue@1: {
xue@1:   int c, p, ip, *rp=new int[N]; for (int i=0; i<N; i++) rp[i]=i;
xue@1:   double m, result=1, *s=new double[N];
xue@1: 
xue@1:   for (int i=0; i<N; i++)
xue@1:   {
xue@1:     s[i]=A[i][0];
xue@1:     for (int j=1; j<N; j++) if (fabs(s[i])<fabs(A[i][j])) s[i]=A[i][j];
xue@1:     if (s[i]==0) {delete[] s; delete[] rp; return 0;}
xue@1:     rp[i]=i;
xue@1:   }
xue@1: 
xue@1:   //Gaussian eliminating
xue@1:   for (int i=0; i<N-1; i++)
xue@1:   {
xue@1:     p=i, ip=i+1;
xue@1:     while (ip<N){if (fabs(A[rp[ip]][i]/s[rp[ip]])>fabs(A[rp[p]][i]/s[rp[p]])) p=ip; ip++;}
xue@1:     if (A[rp[p]][i]==0) {delete[] s; delete[] rp; return 0;}
xue@1:     if (p!=i) {c=rp[i]; rp[i]=rp[p]; rp[p]=c; result=-result;}
xue@1:     result/=A[rp[i]][i];
xue@1:     for (int j=i+1; j<N; j++)
xue@1:     {
xue@1:       m=A[rp[j]][i]/A[rp[i]][i];
xue@1:       A[rp[j]][i]=-m;
xue@1:       for (int k=i+1; k<N; k++) A[rp[j]][k]-=m*A[rp[i]][k];
xue@1:       for (int k=0; k<i; k++) A[rp[j]][k]-=m*A[rp[i]][k];
xue@1:     }
xue@1:   }
xue@1:   if (A[rp[N-1]][N-1]==0) {delete[] s; delete[] rp; return 0;}
xue@1:   result/=A[rp[N-1]][N-1];
xue@1:   //backward substitution
xue@1:   for (int i=0; i<N-1; i++)
xue@1:   {
xue@1:     m=A[rp[i]][i]; for (int k=0; k<N; k++) A[rp[i]][k]/=m; A[rp[i]][i]=1/m;
xue@1:     for (int j=i+1; j<N; j++)
xue@1:     {
xue@1:       m=A[rp[i]][j]/A[rp[j]][j]; for (int k=0; k<N; k++) A[rp[i]][k]-=A[rp[j]][k]*m; A[rp[i]][j]=-m;
xue@1:     }
xue@1:   }
xue@1:   m=A[rp[N-1]][N-1]; for (int k=0; k<N-1; k++) A[rp[N-1]][k]/=m; A[rp[N-1]][N-1]=1/m;
xue@1:   //recover column and row exchange
xue@1:   double* tm=new double[N]; int sizeN=sizeof(double)*N;
xue@1:   for (int i=0; i<N; i++) { for (int j=0; j<N; j++) tm[rp[j]]=A[i][j]; memcpy(A[i], tm, sizeN); }
xue@1:   for (int j=0; j<N; j++) { for (int i=0; i<N; i++) tm[i]=A[rp[i]][j]; for (int i=0; i<N; i++) A[i][j]=tm[i];}
xue@1: 
xue@1:   delete[] tm; delete[] s; delete[] rp;
xue@1:   return result;
xue@1: }//GISCP
xue@1: 
Chris@5: /**
xue@1:   function GISCP: wrapper function that does not overwrite input matrix A: inv(A)->X.
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: matrix X[N][N]
xue@1: 
xue@1:   Returns the determinant of the inverse matrix, 0 on failure.
xue@1: */
xue@1: double GISCP(int N, double** X, double** A)
xue@1: {
xue@1:   Copy(N, X, A);
xue@1:   return GISCP(N, X);
xue@1: }//GISCP
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function GSI: Gaussian-Seidel iterative algorithm for solving linear system Ax=b. Breaks down if any
xue@1:   Aii=0, like the Jocobi method JI(...).
xue@1: 
xue@1:   Gaussian-Seidel iteration is x(k)=(D-L)^(-1)(Ux(k-1)+b), where D is diagonal, L is lower triangular,
xue@1:   U is upper triangular and A=L+D+U.
xue@1: 
xue@1:   In: matrix A[N][N], vector b[N], initial vector x0[N]
xue@1:   Out: vector x0[N]
xue@1: 
xue@1:   Returns 0 is successful. Contents of matrix A and vector b remain unchanged on return.
xue@1: */
xue@1: int GSI(int N, double* x0, double** A, double* b, double ep, int maxiter)
xue@1: {
xue@1:   double e, *x=new double[N];
xue@1:   int k=0, sizeN=sizeof(double)*N;
xue@1:   while (k<maxiter)
xue@1:   {
xue@1:     for (int i=0; i<N; i++)
xue@1:     {
xue@1:       x[i]=b[i];
xue@1:       for (int j=0; j<i; j++) x[i]-=A[i][j]*x[j];
xue@1:       for (int j=i+1; j<N; j++) x[i]-=A[i][j]*x0[j];
xue@1:       x[i]/=A[i][i];
xue@1:     }
xue@1:     e=0; for (int j=0; j<N; j++) e+=fabs(x[j]-x0[j]);
xue@1:     memcpy(x0, x, sizeN);
xue@1:     if (e<ep) break;
xue@1:     k++;
xue@1:   }
xue@1:   delete[] x;
xue@1:   if (k>=maxiter) return 1;
xue@1:   return 0;
xue@1: }//GSI
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function Hessenb: reducing a square matrix A to upper Hessenberg form
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: matrix A[N][N], in upper Hessenberg form
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void Hessenb(int N, double** A)
xue@1: {
xue@1:   double x, y;
xue@1:   for (int m=1; m<N-1; m++)
xue@1:   {
xue@1:     x=0;
xue@1:     int i=m;
xue@1:     for (int j=m; j<N; j++)
xue@1:     {
xue@1:       if (fabs(A[j][m-1]) > fabs(x))
xue@1:       {
xue@1:         x=A[j][m-1];
xue@1:         i=j;
xue@1:       }
xue@1:     }
xue@1:     if (i!=m)
xue@1:     {
xue@1:       for (int j=m-1; j<N; j++)
xue@1:       {
xue@1:         double tmp=A[i][j];
xue@1:         A[i][j]=A[m][j];
xue@1:         A[m][j]=tmp;
xue@1:       }
xue@1:       for (int j=0; j<N; j++)
xue@1:       {
xue@1:         double tmp=A[j][i];
xue@1:         A[j][i]=A[j][m];
xue@1:         A[j][m]=tmp;
xue@1:       }
xue@1:     }
xue@1:     if (x!=0)
xue@1:     {
xue@1:       for (i=m+1; i<N; i++)
xue@1:       {
xue@1:         if ((y=A[i][m-1])!=0)
xue@1:         {
xue@1:           y/=x;
xue@1:           A[i][m-1]=0;
xue@1:           for (int j=m; j<N; j++) A[i][j]-=y*A[m][j];
xue@1:           for (int j=0; j<N; j++) A[j][m]+=y*A[j][i];
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:   }
xue@1: }//Hessenb
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function HouseHolder: house holder method converting a symmetric matrix into a tridiagonal symmetric
xue@1:   matrix, or a non-symmetric matrix into an upper-Hessenberg matrix, using similarity transformation.
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: matrix A[N][N] after transformation
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void HouseHolder(int N, double** A)
xue@1: {
xue@1:   double q, alf, prod, r2, *v=new double[N], *u=new double[N], *z=new double[N];
xue@1:   for (int k=0; k<N-2; k++)
xue@1:   {
xue@1:     q=Inner(N-1-k, &A[k][k+1], &A[k][k+1]);
xue@1: 
xue@1:     if (A[k][k+1]==0) alf=sqrt(q);
xue@1:     else alf=-sqrt(q)*A[k+1][k]/fabs(A[k+1][k]);
xue@1: 
xue@1:     r2=alf*(alf-A[k+1][k]);
xue@1: 
xue@1:     v[k]=0; v[k+1]=A[k][k+1]-alf;
xue@1:     memcpy(&v[k+2], &A[k][k+2], sizeof(double)*(N-k-2));
xue@1: 
xue@1:     for (int j=k; j<N; j++) u[j]=Inner(N-1-k, &A[j][k+1], &v[k+1])/r2;
xue@1: 
xue@1:     prod=Inner(N-1-k, &v[k+1], &u[k+1]);
xue@1: 
xue@1:     MultiAdd(N-k, &z[k], &u[k], &v[k], -prod/2/r2);
xue@1: 
xue@1:     for (int l=k+1; l<N-1; l++)
xue@1:     {
xue@1:       for (int j=l+1; j<N; j++) A[l][j]=A[j][l]=A[j][l]-v[l]*z[j]-v[j]*z[l];
xue@1:       A[l][l]=A[l][l]-2*v[l]*z[l];
xue@1:     }
xue@1: 
xue@1:     A[N-1][N-1]=A[N-1][N-1]-2*v[N-1]*z[N-1];
xue@1: 
xue@1:     for (int j=k+2; j<N; j++) A[k][j]=A[j][k]=0;
xue@1: 
xue@1:     A[k][k+1]=A[k+1][k]=A[k+1][k]-v[k+1]*z[k];
xue@1:   }
xue@1:   delete[] u; delete[] v; delete[] z;
xue@1: }//HouseHolder
xue@1: 
Chris@5: /**
xue@1:   function HouseHolder: house holder transformation T=Q'AQ or A=QTQ', where T is tridiagonal and Q is
xue@1:   unitary i.e. QQ'=I.
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: matrix tridiagonal matrix T[N][N] and unitary matrix Q[N][N]
xue@1: 
xue@1:   No return value. Identical A and T allowed. Content of matrix A is unchanged if A!=T.
xue@1: */
xue@1: void HouseHolder(int N, double** T, double** Q, double** A)
xue@1: {
xue@1:   double g, alf, prod, r2, *v=new double[N], *u=new double[N], *z=new double[N];
xue@1:   int sizeN=sizeof(double)*N;
xue@1:   if (T!=A) for (int i=0; i<N; i++) memcpy(T[i], A[i], sizeN);
xue@1:   for (int i=0; i<N; i++) {memset(Q[i], 0, sizeN); Q[i][i]=1;}
xue@1:   for (int k=0; k<N-2; k++)
xue@1:   {
xue@1:     g=Inner(N-1-k, &T[k][k+1], &T[k][k+1]);
xue@1: 
xue@1:     if (T[k][k+1]==0) alf=sqrt(g);
xue@1:     else alf=-sqrt(g)*T[k+1][k]/fabs(T[k+1][k]);
xue@1: 
xue@1:     r2=alf*(alf-T[k+1][k]);
xue@1: 
xue@1:     v[k]=0; v[k+1]=T[k][k+1]-alf;
xue@1:     memcpy(&v[k+2], &T[k][k+2], sizeof(double)*(N-k-2));
xue@1: 
xue@1:     for (int j=k; j<N; j++) u[j]=Inner(N-1-k, &T[j][k+1], &v[k+1])/r2;
xue@1: 
xue@1:     prod=Inner(N-1-k, &v[k+1], &u[k+1]);
xue@1: 
xue@1:     MultiAdd(N-k, &z[k], &u[k], &v[k], -prod/2/r2);
xue@1: 
xue@1:     for (int l=k+1; l<N-1; l++)
xue@1:     {
xue@1:       for (int j=l+1; j<N; j++) T[l][j]=T[j][l]=T[j][l]-v[l]*z[j]-v[j]*z[l];
xue@1:       T[l][l]=T[l][l]-2*v[l]*z[l];
xue@1:     }
xue@1: 
xue@1:     T[N-1][N-1]=T[N-1][N-1]-2*v[N-1]*z[N-1];
xue@1: 
xue@1:     for (int j=k+2; j<N; j++) T[k][j]=T[j][k]=0;
xue@1: 
xue@1:     T[k][k+1]=T[k+1][k]=T[k+1][k]-v[k+1]*z[k];
xue@1: 
xue@1:     for (int i=0; i<N; i++)
xue@1:       MultiAdd(N-k, &Q[i][k], &Q[i][k], &v[k], -Inner(N-k, &Q[i][k], &v[k])/r2);
xue@1:   }
xue@1:   delete[] u; delete[] v; delete[] z;
xue@1: }//HouseHolder
xue@1: 
Chris@5: /**
xue@1:   function HouseHolder: nr version of householder method for transforming symmetric matrix A to QTQ',
xue@1:   where T is tridiagonal and Q is orthonormal.
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: A[N][N]: now containing Q
xue@1:        d[N]: containing diagonal elements of T
xue@1:        sd[N]: containing subdiagonal elements of T as sd[1:N-1].
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void HouseHolder(int N, double **A, double* d, double* sd)
xue@1: {
xue@1:   for (int i=N-1; i>=1; i--)
xue@1:   {
xue@1:     int l=i-1;
xue@1:     double h=0, scale=0;
xue@1:     if (l>0)
xue@1:     {
xue@1:       for (int k=0; k<=l; k++) scale+=fabs(A[i][k]);
xue@1:       if (scale==0.0) sd[i]=A[i][l];
xue@1:       else
xue@1:       {
xue@1:         for (int k=0; k<=l; k++)
xue@1:         {
xue@1:           A[i][k]/=scale;
xue@1:           h+=A[i][k]*A[i][k];
xue@1:         }
xue@1:         double f=A[i][l];
xue@1:         double g=(f>=0?-sqrt(h): sqrt(h));
xue@1:         sd[i]=scale*g;
xue@1:         h-=f*g;
xue@1:         A[i][l]=f-g;
xue@1:         f=0;
xue@1:         for (int j=0; j<=l; j++)
xue@1:         {
xue@1:           A[j][i]=A[i][j]/h;
xue@1:           g=0;
xue@1:           for (int k=0; k<=j; k++) g+=A[j][k]*A[i][k];
xue@1:           for (int k=j+1; k<=l; k++) g+=A[k][j]*A[i][k];
xue@1:           sd[j]=g/h;
xue@1:           f+=sd[j]*A[i][j];
xue@1:         }
xue@1:         double hh=f/(h+h);
xue@1:         for (int j=0; j<=l; j++)
xue@1:         {
xue@1:           f=A[i][j];
xue@1:           sd[j]=g=sd[j]-hh*f;
xue@1:           for (int k=0; k<=j; k++) A[j][k]-=(f*sd[k]+g*A[i][k]);
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     else
xue@1:       sd[i]=A[i][l];
xue@1:     d[i]=h;
xue@1:   }
xue@1: 
xue@1:   d[0]=sd[0]=0;
xue@1: 
xue@1:   for (int i=0; i<N; i++)
xue@1:   {
xue@1:     int l=i-1;
xue@1:     if (d[i])
xue@1:     {
xue@1:       for (int j=0; j<=l; j++)
xue@1:       {
xue@1:         double g=0.0;
xue@1:         for (int k=0; k<=l; k++) g+=A[i][k]*A[k][j];
xue@1:         for (int k=0; k<=l; k++) A[k][j]-=g*A[k][i];
xue@1:       }
xue@1:     }
xue@1:     d[i]=A[i][i];
xue@1:     A[i][i]=1.0;
xue@1:     for (int j=0; j<=l; j++) A[j][i]=A[i][j]=0.0;
xue@1:   }
xue@1: }//HouseHolder
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function Inner: inner product z=y'x
xue@1: 
xue@1:   In: vectors x[N], y[N]
xue@1: 
xue@1:   Returns inner product of x and y.
xue@1: */
xue@1: double Inner(int N, double* x, double* y)
xue@1: {
xue@1:   double result=0;
xue@1:   for (int i=0; i<N; i++) result+=x[i]*y[i];
xue@1:   return result;
xue@1: }//Inner
xue@1: //complex versions
xue@1: cdouble Inner(int N, double* x, cdouble* y)
xue@1: {
xue@1:   cdouble result=0;
xue@1:   for (int i=0; i<N; i++) result+=x[i]**y[i];
xue@1:   return result;
xue@1: }//Inner
xue@1: cdouble Inner(int N, cdouble* x, cdouble* y)
xue@1: {
xue@1:   cdouble result=0;
xue@1:   for (int i=0; i<N; i++) result+=x[i]^y[i];
xue@1:   return result;
xue@1: }//Inner
xue@1: cdouble Inner(int N, cfloat* x, cdouble* y)
xue@1: {
xue@1:   cdouble result=0;
xue@1:   for (int i=0; i<N; i++) result+=x[i]^y[i];
xue@1:   return result;
xue@1: }//Inner
xue@1: cfloat Inner(int N, cfloat* x, cfloat* y)
xue@1: {
xue@1:   cfloat result=0;
xue@1:   for (int i=0; i<N; i++) result+=x[i]^y[i];
xue@1:   return result;
xue@1: }//Inner
xue@1: 
Chris@5: /**
xue@1:   function Inner: inner product z=tr(Y'X)
xue@1: 
xue@1:   In: matrices X[M][N], Y[M][N]
xue@1: 
xue@1:   Returns inner product of X and Y.
xue@1: */
xue@1: double Inner(int M, int N, double** X, double** Y)
xue@1: {
xue@1: 	double result=0;
xue@1:   for (int m=0; m<M; m++) for (int n=0; n<N; n++) result+=X[m][n]*Y[m][n];
xue@1: 	return result;
xue@1: }//Inner
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function JI: Jacobi interative algorithm for solving linear system Ax=b Breaks down if A[i][i]=0 for
xue@1:   any i. Reorder A so that this does not happen.
xue@1: 
xue@1:   Jacobi iteration is x(k)=D^(-1)((L+U)x(k-1)+b), D is diagonal, L is lower triangular, U is upper
xue@1:   triangular and A=L+D+U.
xue@1: 
xue@1:   In: matrix A[N][N], vector b[N], initial vector x0[N]
xue@1:   Out: vector x0[N]
xue@1: 
xue@1:   Returns 0 if successful. Contents of matrix A and vector b are unchanged on return.
xue@1: */
xue@1: int JI(int N, double* x0, double** A, double* b, double ep, int maxiter)
xue@1: {
xue@1:   double e, *x=new double[N];
xue@1:   int k=0, sizeN=sizeof(double)*N;
xue@1:   while (k<maxiter)
xue@1:   {
xue@1:     for (int i=0; i<N; i++)
xue@1:     {
xue@1:       x[i]=b[i]; for (int j=0; j<N; j++) if (j!=i) x[i]-=A[i][j]*x0[j]; x[i]=x[i]/A[i][i];
xue@1:     }
xue@1:     e=0; for (int j=0; j<N; j++) e+=fabs(x[j]-x0[j]); //inf-norm used here
xue@1:     memcpy(x0, x, sizeN);
xue@1:     if (e<ep) break;
xue@1:     k++;
xue@1:   }
xue@1:   delete[] x;
xue@1:   if (k>=maxiter) return 1;
xue@1:   else return 0;
xue@1: }//JI
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function LDL: LDL' decomposition A=LDL', where L is lower triangular and D is diagonal identical l and
xue@1:   a allowed.
xue@1: 
xue@1:   The symmetric matrix A is positive definite iff A can be factorized as LDL', where L is lower
xue@1:   triangular with ones on its diagonal and D is diagonal with positive diagonal entries.
xue@1: 
xue@1:   If a symmetric matrix A can be reduced by Gaussian elimination without row interchanges, then it can
xue@1:   be factored into LDL', where L is lower triangular with ones on its diagonal and D is diagonal with
xue@1:   non-zero diagonal entries.
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: lower triangular matrix L[N][N], vector d[N] containing diagonal elements of D
xue@1: 
xue@1:   Returns 0 if successful. Content of matrix A is unchanged on return.
xue@1: */
xue@1: int LDL(int N, double** L, double* d, double** A)
xue@1: {
xue@1:   double* v=new double[N];
xue@1: 
xue@1:   if (A[0][0]==0) {delete[] v; return 1;}
xue@1:   d[0]=A[0][0]; for (int j=1; j<N; j++) L[j][0]=A[j][0]/d[0];
xue@1:   for (int i=1; i<N; i++)
xue@1:   {
xue@1:     for (int j=0; j<i; j++) v[j]=L[i][j]*d[j];
xue@1:     d[i]=A[i][i]; for (int j=0; j<i; j++) d[i]-=L[i][j]*v[j];
xue@1:     if (d[i]==0) {delete[] v; return 1;}
xue@1:     for (int j=i+1; j<N; j++)
xue@1:     {
xue@1:       L[j][i]=A[j][i]; for (int k=0; k<i; k++) L[j][i]-=L[j][k]*v[k]; L[j][i]/=d[i];
xue@1:     }
xue@1:   }
xue@1:   delete[] v;
xue@1: 
xue@1:   for (int i=0; i<N; i++) {L[i][i]=1; memset(&L[i][i+1], 0, sizeof(double)*(N-1-i));}
xue@1:   return 0;
xue@1: }//LDL
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function LQ_GS: LQ decomposition using Gram-Schmidt method
xue@1: 
xue@1:   In: matrix A[M][N], M<=N
xue@1:   Out: matrices L[M][M], Q[M][N]
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void LQ_GS(int M, int N, double** A, double** L, double** Q)
xue@1: {
xue@1:   double *u=new double[N];
xue@1:   for (int m=0; m<M; m++)
xue@1:   {
xue@1:     memset(L[m], 0, sizeof(double)*M);
xue@1:     memcpy(u, A[m], sizeof(double)*N);
xue@1:     for (int k=0; k<m; k++)
xue@1:     {
xue@1:       double ip=0; for (int n=0; n<N; n++) ip+=Q[k][n]*u[n];
xue@1:       for (int n=0; n<N; n++) u[n]-=ip*Q[k][n];
xue@1:       L[m][k]=ip;
xue@1:     }
xue@1:     double iu=0; for (int n=0; n<N; n++) iu+=u[n]*u[n]; iu=sqrt(iu);
xue@1:     L[m][m]=iu; iu=1.0/iu;
xue@1:     for (int n=0; n<N; n++) Q[m][n]=u[n]*iu;
xue@1:   }
xue@1:   delete[] u;
xue@1: }//LQ_GS
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function LSLinear2: 2-dtage LS solution of A[M][N]x[N][1]=y[M][1], M>=N. Use of this function requires
xue@1:   the submatrix A[N][N] be invertible.
xue@1: 
xue@1:   In: matrix A[M][N], vector y[M], M>=N.
xue@1:   Out: vector x[N].
xue@1: 
xue@1:   No return value. Contents of matrix A and vector y are unchanged on return.
xue@1: */
xue@1: void LSLinear2(int M, int N, double* x, double** A, double* y)
xue@1: {
xue@1:   double** A1=Copy(N, N, 0, A);
xue@1:   LU(N, x, A1, y);
xue@1:   if (M>N)
xue@1:   {
xue@1:     double** B=&A[N];
xue@1:     double* Del=MultiplyXy(M-N, N, B, x);
xue@1:     MultiAdd(M-N, Del, Del, &y[N], -1);
xue@1:     double** A2=MultiplyXtX(N, N, A);
xue@1:     MultiplyXtX(N, M-N, A1, B);
xue@1:     MultiAdd(N, N, A2, A2, A1, 1);
xue@1:     double* b2=MultiplyXty(N, M-N, B, Del);
xue@1:     double* dx=new double[N];
xue@1:     GESCP(N, dx, A2, b2);
xue@1:     MultiAdd(N, x, x, dx, -1);
xue@1:     delete[] dx;
xue@1:     delete[] Del;
xue@1:     delete[] b2;
xue@1:     DeAlloc2(A2);
xue@1:   }
xue@1:   DeAlloc2(A1);
xue@1: }//LSLinear2
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function LU: LU decomposition A=LU, where L is lower triangular with diagonal entries 1 and U is upper
xue@1:   triangular.
xue@1: 
xue@1:   LU is possible if A can be reduced by Gaussian elimination without row interchanges.
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: matrices L[N][N] and U[N][N], subject to input values of L and U:
xue@1:         if L euqals NULL, L is not returned
xue@1:         if U equals NULL or A, U is returned in A, s.t. A is modified
xue@1:         if L equals A, L is returned in A, s.t. A is modified
xue@1:         if L equals U, L and U are returned in the same matrix
xue@1:         when L and U are returned in the same matrix, diagonal of L (all 1) is not returned
xue@1: 
xue@1:   Returns 0 if successful.
xue@1: */
xue@1: int LU(int N, double** L, double** U, double** A)
xue@1: {
xue@1:   double* diagl=new double[N];
xue@1:   for (int i=0; i<N; i++) diagl[i]=1;
xue@1: 
xue@1:   int sizeN=sizeof(double)*N;
xue@1:   if (U==0) U=A;
xue@1:   if (U!=A) for (int i=0; i<N; i++) memcpy(U[i], A[i], sizeN);
xue@1:   int result=LU_Direct(0, N, diagl, U);
xue@1:   if (result==0)
xue@1:   {
xue@1:     if (L!=U)
xue@1:     {
xue@1:       if (L!=0) for (int i=0; i<N; i++) {memcpy(L[i], U[i], sizeof(double)*i); L[i][i]=1; memset(&L[i][i+1], 0, sizeof(double)*(N-i-1));}
xue@1:       for (int i=1; i<N; i++) memset(U[i], 0, sizeof(double)*i);
xue@1:     }
xue@1:   }
xue@1:   delete[] diagl;
xue@1:   return result;
xue@1: }//LU
xue@1: 
Chris@5: /**
xue@1:   function LU: Solving linear system Ax=y by LU factorization
xue@1: 
xue@1:   In: matrix A[N][N], vector y[N]
xue@1:   Out: x[N]
xue@1: 
xue@1:   No return value. On return A contains its LU factorization (with pivoting, diag mode 1), y remains
xue@1:   unchanged.
xue@1: */
xue@1: void LU(int N, double* x, double** A, double* y, int* ind)
xue@1: {
xue@1:   int parity;
xue@1:   bool allocind=!ind;
xue@1:   if (allocind) ind=new int[N];
xue@1:   LUCP(A, N, ind, parity, 1);
xue@1:   for (int i=0; i<N; i++) x[i]=y[ind[i]];
xue@1:   for (int i=0; i<N; i++)
xue@1:   {
xue@1:     for (int j=i+1; j<N; j++) x[j]=x[j]-x[i]*A[j][i];
xue@1:   }
xue@1:   for (int i=N-1; i>=0; i--)
xue@1:   {
xue@1:     x[i]/=A[i][i];
xue@1:     for (int j=0; j<i; j++) x[j]=x[j]-x[i]*A[j][i];
xue@1:   }
xue@1:   if (allocind) delete[] ind;
xue@1: }//LU
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: /*
xue@1:   LU_DiagL shows the original procedure for calculating A=LU in separate buffers substitute l and u by a
xue@1:   gives the stand-still method LU_Direct().
xue@1: *//*
xue@1: void LU_DiagL(int N, double** l, double* diagl, double** u, double** a)
xue@1: {
xue@1:   l[0][0]=diagl[0]; u[0][0]=a[0][0]/l[0][0]; //here to signal failure if l[00]u[00]=0
xue@1:   for (int j=1; j<N; j++) u[0][j]=a[0][j]/l[0][0], l[j][0]=a[j][0]/u[0][0];
xue@1:   memset(&l[0][1], 0, sizeof(double)*(N-1));
xue@1:   for (int i=1; i<N-1; i++)
xue@1:   {
xue@1:     l[i][i]=diagl[i];
xue@1:     u[i][i]=a[i][i]; for (int k=0; k<i; k++) u[i][i]-=l[i][k]*u[k][i]; u[i][i]/=l[i][i]; //here to signal failure if l[ii]u[ii]=0
xue@1:     for (int j=i+1; j<N; j++)
xue@1:     {
xue@1:       u[i][j]=a[i][j]; for (int k=0; k<i; k++) u[i][j]-=l[i][k]*u[k][j]; u[i][j]/=l[i][i];
xue@1:       l[j][i]=a[j][i]; for (int k=0; k<i; k++) l[j][i]-=l[j][k]*u[k][i]; l[j][i]/=u[i][i];
xue@1:     }
xue@1:     memset(&l[i][i+1], 0, sizeof(double)*(N-1-i)), memset(u[i], 0, sizeof(double)*i);
xue@1:   }
xue@1:   l[N-1][N-1]=diagl[N-1];
xue@1:   u[N-1][N-1]=a[N-1][N-1]; for (int k=0; k<N-1; k++) u[N-1][N-1]-=l[N-1][k]*u[k][N-1]; u[N-1][N-1]/=l[N-1][N-1];
xue@1:   memset(u[N-1], 0, sizeof(double)*(N-1));
xue@1: } //LU_DiagL*/
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function LU_Direct: LU factorization A=LU.
xue@1: 
xue@1:   In: matrix A[N][N], vector diag[N] specifying main diagonal of L or U, according to mode (0=LDiag,
xue@1:         1=UDiag).
xue@1:   Out: matrix A[N][N] now containing L and U.
xue@1: 
xue@1:   Returns 0 if successful.
xue@1: */
xue@1: int LU_Direct(int mode, int N, double* diag, double** A)
xue@1: {
xue@1:   if (mode==0)
xue@1:   {
xue@1:     if (A[0][0]==0) return 1;
xue@1:     A[0][0]=A[0][0]/diag[0];
xue@1:     for (int j=1; j<N; j++) A[0][j]=A[0][j]/diag[0], A[j][0]=A[j][0]/A[0][0];
xue@1:     for (int i=1; i<N-1; i++)
xue@1:     {
xue@1:       for (int k=0; k<i; k++) A[i][i]-=A[i][k]*A[k][i]; A[i][i]/=diag[i];
xue@1:       if (A[i][i]==0) return 2;
xue@1:       for (int j=i+1; j<N; j++)
xue@1:       {
xue@1:         for (int k=0; k<i; k++) A[i][j]-=A[i][k]*A[k][j]; A[i][j]/=diag[i];
xue@1:         for (int k=0; k<i; k++) A[j][i]-=A[j][k]*A[k][i]; A[j][i]/=A[i][i];
xue@1:       }
xue@1:     }
xue@1:     for (int k=0; k<N-1; k++) A[N-1][N-1]-=A[N-1][k]*A[k][N-1]; A[N-1][N-1]/=diag[N-1];
xue@1:   }
xue@1:   else if (mode==1)
xue@1:   {
xue@1:     A[0][0]=A[0][0]/diag[0];
xue@1:     if (A[0][0]==0) return 1;
xue@1:     for (int j=1; j<N; j++) A[0][j]=A[0][j]/A[0][0], A[j][0]=A[j][0]/diag[0];
xue@1:     for (int i=1; i<N-1; i++)
xue@1:     {
xue@1:       for (int k=0; k<i; k++) A[i][i]-=A[i][k]*A[k][i]; A[i][i]/=diag[i];
xue@1:       if (A[i][i]==0) return 2;
xue@1:       for (int j=i+1; j<N; j++)
xue@1:       {
xue@1:         for (int k=0; k<i; k++) A[i][j]-=A[i][k]*A[k][j]; A[i][j]/=A[i][i];
xue@1:         for (int k=0; k<i; k++) A[j][i]-=A[j][k]*A[k][i]; A[j][i]/=diag[i];
xue@1:       }
xue@1:     }
xue@1:     for (int k=0; k<N-1; k++) A[N-1][N-1]-=A[N-1][k]*A[k][N-1]; A[N-1][N-1]/=diag[N-1];
xue@1:   }
xue@1:   return 0;
xue@1: }//LU_Direct
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function LU_PD: LU factorization for pentadiagonal A=LU
xue@1: 
xue@1:   In: pentadiagonal matrix A[N][N] stored in a compact format, i.e. A[i][j]->b[i-j, j]
xue@1:         the main diagonal is b[0][0]~b[0][N-1]
xue@1:         the 1st upper subdiagonal is b[-1][1]~b[-1][N-1]
xue@1:         the 2nd upper subdiagonal is b[-2][2]~b[-2][N-1]
xue@1:         the 1st lower subdiagonal is b[1][0]~b[1][N-2]
xue@1:         the 2nd lower subdiagonal is b[2][0]~b[2][N-3]
xue@1: 
xue@1:   Out: L[N][N] and U[N][N], main diagonal of L being all 1 (probably), stored in a compact format in
xue@1:         b[-2:2][N].
xue@1: 
xue@1:   Returns 0 if successful.
xue@1: */
xue@1: int LU_PD(int N, double** b)
xue@1: {
xue@1:   if (b[0][0]==0) return 1;
xue@1:   b[1][0]/=b[0][0], b[2][0]/=b[0][0];
xue@1: 
xue@1:     //i=1, not to double b[*][i-2], b[-2][i]
xue@1:     b[0][1]-=b[1][0]*b[-1][1];
xue@1:     if (b[0][1]==0) return 2;
xue@1:     b[-1][2]-=b[1][0]*b[-2][2];
xue@1:     b[1][1]-=b[2][0]*b[-1][1];
xue@1:     b[1][1]/=b[0][1];
xue@1:     b[2][1]/=b[0][1];
xue@1: 
xue@1:   for (int i=2; i<N-2; i++)
xue@1:   {
xue@1:     b[0][i]-=b[2][i-2]*b[-2][i];
xue@1:     b[0][i]-=b[1][i-1]*b[-1][i];
xue@1:     if (b[0][i]==0) return 2;
xue@1:     b[-1][i+1]-=b[1][i-1]*b[-2][i+1];
xue@1:     b[1][i]-=b[2][i-1]*b[-1][i];
xue@1:     b[1][i]/=b[0][i];
xue@1:     b[2][i]/=b[0][i];
xue@1:   }
xue@1:     //i=N-2, not to tough b[2][i]
xue@1:     b[0][N-2]-=b[2][N-4]*b[-2][N-2];
xue@1:     b[0][N-2]-=b[1][N-3]*b[-1][N-2];
xue@1:     if (b[0][N-2]==0) return 2;
xue@1:     b[-1][N-1]-=b[1][N-3]*b[-2][N-1];
xue@1:     b[1][N-2]-=b[2][N-3]*b[-1][N-2];
xue@1:     b[1][N-2]/=b[0][N-2];
xue@1: 
xue@1:   b[0][N-1]-=b[2][N-3]*b[-2][N-1];
xue@1:   b[0][N-1]-=b[1][N-2]*b[-1][N-1];
xue@1:   return 0;
xue@1: }//LU_PD
xue@1: 
xue@1: /*
xue@1:   This old version is kept here as a reference.
xue@1: *//*
xue@1: int LU_PD(int N, double** b)
xue@1: {
xue@1:   if (b[0][0]==0) return 1;
xue@1:   for (int j=1; j<3; j++) b[j][0]=b[j][0]/b[0][0];
xue@1:   for (int i=1; i<N-1; i++)
xue@1:   {
xue@1:     for (int k=i-2; k<i; k++) b[0][i]-=b[i-k][k]*b[k-i][i];
xue@1:     if (b[0][i]==0) return 2;
xue@1:     for (int j=i+1; j<i+3; j++)
xue@1:     {
xue@1:       for (int k=j-2; k<i; k++) b[i-j][j]-=b[i-k][k]*b[k-j][j];
xue@1:       for (int k=j-2; k<i; k++) b[j-i][i]-=b[j-k][k]*b[k-i][i];
xue@1:       b[j-i][i]/=b[0][i];
xue@1:     }
xue@1:   }
xue@1:   for (int k=N-3; k<N-1; k++) b[0][N-1]-=b[N-1-k][k]*b[k-N+1][N-1];
xue@1:   return 0;
xue@1: }//LU_PD*/
xue@1: 
Chris@5: /**
xue@1:   function LU_PD: solve pentadiagonal system Ax=c
xue@1: 
xue@1:   In: pentadiagonal matrix A[N][N] stored in a compact format in b[-2:2][N], vector c[N]
xue@1:   Out: vector c now containing x.
xue@1: 
xue@1:   Returns 0 if successful. On return b is in the LU form.
xue@1: */
xue@1: int LU_PD(int N, double** b, double* c)
xue@1: {
xue@1:   int result=LU_PD(N, b);
xue@1:   if (result==0)
xue@1:   {
xue@1:     //L loop
xue@1:     c[1]=c[1]-b[1][0]*c[0];
xue@1:     for (int i=2; i<N; i++)
xue@1:       c[i]=c[i]-b[1][i-1]*c[i-1]-b[2][i-2]*c[i-2];
xue@1:     //U loop
xue@1:     c[N-1]/=b[0][N-1];
xue@1:     c[N-2]=(c[N-2]-b[-1][N-1]*c[N-1])/b[0][N-2];
xue@1:     for (int i=N-3; i>=0; i--)
xue@1:       c[i]=(c[i]-b[-1][i+1]*c[i+1]-b[-2][i+2]*c[i+2])/b[0][i];
xue@1:   }
xue@1:   return result;
xue@1: }//LU_PD
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function LUCP: LU decomposition A=LU with column pivoting
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: matrix A[N][N] now holding L and U by L_U[i][j]=A[ind[i]][j], where L_U
xue@1:         hosts L and U according to mode:
xue@1:         mode=0: L diag=abs(U diag), U diag as return
xue@1:         mode=1: L diag=1, U diag as return
xue@1:         mode=2: U diag=1, L diag as return
xue@1: 
xue@1:   Returns the determinant of A.
xue@1: */
xue@1: double LUCP(double **A, int N, int *ind, int &parity, int mode)
xue@1: {
xue@1:   double det=1;
xue@1:   parity=1;
xue@1: 
xue@1:   for (int i=0; i<N; i++) ind[i]=i;
xue@1:   double vmax, *norm=new double[N]; //norm[n] is the maxima of row n
xue@1:   for (int i=0; i<N; i++)
xue@1:   {
xue@1:     vmax=fabs(A[i][0]);
xue@1:     double tmp;
xue@1:     for (int j=1; j<N; j++) if ((tmp=fabs(A[i][j]))>vmax) vmax=tmp;
xue@1:     if (vmax==0) { parity=0; goto deletenorm; } //det=0 at this point
xue@1:     norm[i]=1/vmax;
xue@1:   }
xue@1: 
xue@1:   int maxind;
xue@1:   for (int j=0; j<N; j++)
xue@1:   { //Column j
xue@1:     for (int i=0; i<j; i++)
xue@1:     {
xue@1:       //row i, i<j
xue@1:       double tmp=A[i][j];
xue@1:       for (int k=0; k<i; k++) tmp-=A[i][k]*A[k][j];
xue@1:       A[i][j]=tmp;
xue@1:     }
xue@1:     for (int i=j; i<N; i++)
xue@1:     {
xue@1:       //row i, i>=j
xue@1:       double tmp=A[i][j]; for (int k=0; k<j; k++) tmp-=A[i][k]*A[k][j]; A[i][j]=tmp;
xue@1:       double tmp2=norm[i]*fabs(tmp);
xue@1:       if (i==j || tmp2>=vmax) maxind=i, vmax=tmp2;
xue@1:     }
xue@1:     if (vmax==0) { parity=0; goto deletenorm; } //pivot being zero
xue@1:     if (j!=maxind)
xue@1:     {
xue@1:       //do column pivoting: switching rows
xue@1:       for (int k=0; k<N; k++) { double tmp=A[maxind][k]; A[maxind][k]=A[j][k]; A[j][k]=tmp; }
xue@1:       parity=-parity;
xue@1:       norm[maxind]=norm[j];
xue@1:     }
xue@1:     int itmp=ind[j]; ind[j]=ind[maxind]; ind[maxind]=itmp;
xue@1:     if (j!=N-1)
xue@1:     {
xue@1:       double den=1/A[j][j];
xue@1:       for (int i=j+1; i<N; i++) A[i][j]*=den;
xue@1:     }
xue@1:     det*=A[j][j];
xue@1:   } //Go back for the next column in the reduction.
xue@1: 
xue@1:   if (mode==0)
xue@1:   {
xue@1:     for (int i=0; i<N-1; i++)
xue@1:     {
xue@1:       double den=sqrt(fabs(A[i][i]));
xue@1:       double iden=1/den;
xue@1:       for (int j=i+1; j<N; j++) A[j][i]*=den, A[i][j]*=iden;
xue@1:       A[i][i]*=iden;
xue@1:     }
xue@1:     A[N-1][N-1]/=sqrt(fabs(A[N-1][N-1]));
xue@1:   }
xue@1:   else if (mode==2)
xue@1:   {
xue@1:     for (int i=0; i<N-1; i++)
xue@1:     {
xue@1:       double den=A[i][i];
xue@1:       double iden=1/den;
xue@1:       for (int j=i+1; j<N; j++) A[j][i]*=den, A[i][j]*=iden;
xue@1:     }
xue@1:   }
xue@1: 
xue@1: deletenorm:
xue@1:   delete[] norm;
xue@1:   return det*parity;
xue@1: }//LUCP
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function maxind: returns the index of the maximal value of data[from:(to-1)].
xue@1: 
xue@1:   In: vector data containing at least $to entries.
xue@1:   Out: the index to the maximal entry of data[from:(to-1)]
xue@1: 
xue@1:   Returns the index to the maximal value.
xue@1: */
xue@1: int maxind(double* data, int from, int to)
xue@1: {
xue@1:   int result=from;
xue@1:   for (int i=from+1; i<to; i++) if (data[result]<data[i]) result=i;
xue@1:   return result;
xue@1: }//maxind
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: /*
xue@1:   macro Multiply_vect: matrix-vector multiplications
xue@1: 
xue@1:   Each expansion of this macro implements two functions named $MULTIPLY that do matrix-vector
xue@1:   multiplication. Functions are named after their exact functions. For example, MultiplyXty() does
xue@1:   multiplication of the transpose of matrix X with vector y, where postfix "t" attched to Y stands for
xue@1:   transpose. Likewise, the postfix "c" stands for conjugate, and "h" stnads for Hermitian (conjugate
xue@1:   transpose).
xue@1: 
xue@1:   Two dimension arguments are needed by each function. The first of the two is the number of entries to
xue@1:   the output vector; the second of the two is the "other" dimension of the matrix multiplier.
xue@1: */
xue@1: #define Multiply_vect(MULTIPLY, DbZ, DbX, DbY, xx, yy) \
xue@1:   DbZ* MULTIPLY(int M, int N, DbZ* z, DbX* x, DbY* y, MList* List) \
xue@1:   { \
xue@1:     if (!z){z=new DbZ[M]; if (List) List->Add(z, 1);} \
xue@1:     for (int m=0; m<M; m++){z[m]=0; for (int n=0; n<N; n++) z[m]+=xx*yy;} \
xue@1:     return z; \
xue@1:   } \
xue@1:   DbZ* MULTIPLY(int M, int N, DbX* x, DbY* y, MList* List) \
xue@1:   { \
xue@1:     DbZ* z=new DbZ[M]; if (List) List->Add(z, 1); \
xue@1:     for (int m=0; m<M; m++){z[m]=0; for (int n=0; n<N; n++) z[m]+=xx*yy;} \
xue@1:     return z; \
xue@1:   }
xue@1: //function MultiplyXy: z[M]=x[M][N]y[N], identical z and y NOT ALLOWED
xue@1: Multiply_vect(MultiplyXy, double, double*, double, x[m][n], y[n])
xue@1: Multiply_vect(MultiplyXy, cdouble, cdouble*, cdouble, x[m][n], y[n])
xue@1: Multiply_vect(MultiplyXy, cdouble, double*, cdouble, x[m][n], y[n])
xue@1: //function MultiplyxY: z[M]=x[N]y[N][M], identical z and x NOT ALLOWED
xue@1: Multiply_vect(MultiplyxY, double, double, double*, x[n], y[n][m])
xue@1: Multiply_vect(MultiplyxY, cdouble, cdouble, cdouble*, x[n], y[n][m])
xue@1: //function MultiplyXty: z[M]=xt[M][N]y[N]
xue@1: Multiply_vect(MultiplyXty, double, double*, double, x[n][m], y[n])
xue@1: Multiply_vect(MultiplyXty, cdouble, cdouble*, cdouble, x[n][m], y[n])
xue@1: //function MultiplyXhy: z[M]=xh[M][N]y[N]
xue@1: Multiply_vect(MultiplyXhy, cdouble, cdouble*, cdouble, *x[n][m], y[n])
xue@1: //function MultiplyxYt: z[M]=x[N]yt[N][M]
xue@1: Multiply_vect(MultiplyxYt, double, double, double*, x[n], y[m][n])
xue@1: //function MultiplyXcy: z[M]=(x*)[M][N]y[N]
xue@1: Multiply_vect(MultiplyXcy, cdouble, cdouble*, cdouble, *x[m][n], y[n])
xue@1: Multiply_vect(MultiplyXcy, cdouble, cdouble*, cfloat, *x[m][n], y[n])
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function Norm1: L-1 norm of a square matrix A
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: its L-1 norm
xue@1: 
xue@1:   Returns the L-1 norm.
xue@1: */
xue@1: double Norm1(int N, double** A)
xue@1: {
xue@1:   double result=0, norm;
xue@1:   for (int i=0; i<N; i++)
xue@1:   {
xue@1:     norm=0; for (int j=0; j<N; j++) norm+=fabs(A[i][j]);
xue@1:     if (result<norm) result=norm;
xue@1:   }
xue@1:   return result;
xue@1: }//Norm1
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function QL: QL method for solving tridiagonal symmetric matrix eigenvalue problem.
xue@1: 
xue@1:   In: A[N][N]: tridiagonal symmetric matrix stored in d[N] and sd[] arranged so that d[0:n-1] contains
xue@1:         the diagonal elements of A, sd[0]=0, sd[1:n-1] contains the subdiagonal elements of A.
xue@1:       z[N][N]: pre-transform matrix z[N][N] compatible with HouseHolder() routine.
xue@1:   Out: d[N]: the eigenvalues of A
xue@1:        z[N][N] the eigenvectors of A.
xue@1: 
xue@1:   Returns 0 if successful. sd[] should have storage for at least N+1 entries.
xue@1: */
xue@1: int QL(int N, double* d, double* sd, double** z)
xue@1: {
xue@1:   const int maxiter=30;
xue@1:   for (int i=1; i<N; i++) sd[i-1]=sd[i];
xue@1:   sd[N]=0.0;
xue@1:   for (int l=0; l<N; l++)
xue@1:   {
xue@1:     int iter=0, m;
xue@1:     do
xue@1:     {
xue@1:       for (m=l; m<N-1; m++)
xue@1:       {
xue@1:         double dd=fabs(d[m])+fabs(d[m+1]);
xue@1:         if (fabs(sd[m])+dd==dd) break;
xue@1:       }
xue@1:       if (m!=l)
xue@1:       {
xue@1:         iter++;
xue@1:         if (iter>=maxiter) return 1;
xue@1:         double g=(d[l+1]-d[l])/(2*sd[l]);
xue@1:         double r=sqrt(g*g+1);
xue@1:         g=d[m]-d[l]+sd[l]/(g+(g>=0?r:-r));
xue@1:         double s=1, c=1, p=0;
xue@1:         int i;
xue@1:         for (i=m-1; i>=l; i--)
xue@1:         {
xue@1:           double f=s*sd[i], b=c*sd[i];
xue@1:           sd[i+1]=(r=sqrt(f*f+g*g));
xue@1:           if (r==0)
xue@1:           {
xue@1:             d[i+1]-=p;
xue@1:             sd[m]=0;
xue@1:             break;
xue@1:           }
xue@1:           s=f/r, c=g/r;
xue@1:           g=d[i+1]-p;
xue@1:           r=(d[i]-g)*s+2.0*c*b;
xue@1:           p=s*r;
xue@1:           d[i+1]=g+p;
xue@1:           g=c*r-b;
xue@1:           for (int k=0; k<N; k++)
xue@1:           {
xue@1:             f=z[k][i+1];
xue@1:             z[k][i+1]=s*z[k][i]+c*f;
xue@1:             z[k][i]=c*z[k][i]-s*f;
xue@1:           }
xue@1:         }
xue@1:         if (r==0 && i>=l) continue;
xue@1:         d[l]-=p;
xue@1:         sd[l]=g;
xue@1:         sd[m]=0.0;
xue@1:       }
xue@1:     }
xue@1:     while (m!=l);
xue@1:   }
xue@1:   return 0;
xue@1: }//QL
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function QR: nr version of QR method for solving upper Hessenberg system A. This is compatible with
xue@1:   Hessenb method.
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: vector ev[N] of eigenvalues
xue@1: 
xue@1:   Returns 0 on success. Content of matrix A is destroyed on return.
xue@1: */
xue@1: int QR(int N, double **A, cdouble* ev)
xue@1: {
xue@1:   int n=N, m, l, k, j, iter, i, mmin, maxiter=30;
xue@1:   double **a=A, z, y, x, w, v, u, t=0, s, r, q, p, a1=0;
xue@1:   for (i=0; i<n; i++) for (j=i-1>0?i-1:0; j<n; j++) a1+=fabs(a[i][j]);
xue@1:   n--;
xue@1:   while (n>=0)
xue@1:   {
xue@1:     iter=0;
xue@1:     do
xue@1:     {
xue@1:       for (l=n; l>0; l--)
xue@1:       {
xue@1:         s=fabs(a[l-1][l-1])+fabs(a[l][l]);
xue@1:         if (s==0) s=a1;
xue@1:         if (fabs(a[l][l-1])+s==s) {a[l][l-1]=0; break;}
xue@1:       }
xue@1:       x=a[n][n];
xue@1:       if (l==n) {ev[n].x=x+t; ev[n--].y=0;}
xue@1:       else
xue@1:       {
xue@1:         y=a[n-1][n-1], w=a[n][n-1]*a[n-1][n];
xue@1:         if (l==(n-1))
xue@1:         {
xue@1:           p=0.5*(y-x);
xue@1:           q=p*p+w;
xue@1:           z=sqrt(fabs(q));
xue@1:           x+=t;
xue@1:           if (q>=0)
xue@1:           {
xue@1:             z=p+(p>=0?z:-z);
xue@1:             ev[n-1].x=ev[n].x=x+z;
xue@1:             if (z) ev[n].x=x-w/z;
xue@1:             ev[n-1].y=ev[n].y=0;
xue@1:           }
xue@1:           else
xue@1:           {
xue@1:             ev[n-1].x=ev[n].x=x+p;
xue@1:             ev[n].y=z; ev[n-1].y=-z;
xue@1:           }
xue@1:           n-=2;
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           if (iter>=maxiter) return 1;
xue@1:           if (iter%10==9)
xue@1:           {
xue@1:             t+=x;
xue@1:             for (i=0; i<=n; i++) a[i][i]-=x;
xue@1:             s=fabs(a[n][n-1])+fabs(a[n-1][n-2]);
xue@1:             y=x=0.75*s;
xue@1:             w=-0.4375*s*s;
xue@1:           }
xue@1:           iter++;
xue@1:           for (m=n-2; m>=l; m--)
xue@1:           {
xue@1:             z=a[m][m];
xue@1:             r=x-z; s=y-z;
xue@1:             p=(r*s-w)/a[m+1][m]+a[m][m+1]; q=a[m+1][m+1]-z-r-s; r=a[m+2][m+1];
xue@1:             s=fabs(p)+fabs(q)+fabs(r);
xue@1:             p/=s; q/=s; r/=s;
xue@1:             if (m==l) break;
xue@1:             u=fabs(a[m][m-1])*(fabs(q)+fabs(r));
xue@1:             v=fabs(p)*(fabs(a[m-1][m-1])+fabs(z)+fabs(a[m+1][m+1]));
xue@1:             if (u+v==v) break;
xue@1:           }
xue@1:           for (i=m+2; i<=n; i++)
xue@1:           {
xue@1:             a[i][i-2]=0;
xue@1:             if (i!=m+2) a[i][i-3]=0;
xue@1:           }
xue@1:           for (k=m; k<=n-1; k++)
xue@1:           {
xue@1:             if (k!=m)
xue@1:             {
xue@1:               p=a[k][k-1];
xue@1:               q=a[k+1][k-1];
xue@1:               r=0;
xue@1:               if (k!=n-1) r=a[k+2][k-1];
xue@1:               x=fabs(p)+fabs(q)+fabs(r);
xue@1:               if (x!=0) p/=x, q/=x, r/=x;
xue@1:             }
xue@1:             if (p>=0) s=sqrt(p*p+q*q+r*r);
xue@1:             else s=-sqrt(p*p+q*q+r*r);
xue@1:             if (s!=0)
xue@1:             {
xue@1:               if (k==m)
xue@1:               {
xue@1:                 if (l!=m) a[k][k-1]=-a[k][k-1];
xue@1:               }
xue@1:               else a[k][k-1]=-s*x;
xue@1:               p+=s;
xue@1:               x=p/s; y=q/s; z=r/s; q/=p; r/=p;
xue@1:               for (j=k; j<=n; j++)
xue@1:               {
xue@1:                 p=a[k][j]+q*a[k+1][j];
xue@1:                 if (k!=n-1)
xue@1:                 {
xue@1:                   p+=r*a[k+2][j];
xue@1:                   a[k+2][j]-=p*z;
xue@1:                 }
xue@1:                 a[k+1][j]-=p*y; a[k][j]-=p*x;
xue@1:               }
xue@1:               mmin=n<k+3?n:k+3;
xue@1:               for (i=l; i<=mmin; i++)
xue@1:               {
xue@1:                 p=x*a[i][k]+y*a[i][k+1];
xue@1:                 if (k!=(n-1))
xue@1:                 {
xue@1:                   p+=z*a[i][k+2];
xue@1:                   a[i][k+2]-=p*r;
xue@1:                 }
xue@1:                 a[i][k+1]-=p*q; a[i][k]-=p;
xue@1:               }
xue@1:             }
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:     } while (n>l+1);
xue@1:   }
xue@1:   return 0;
xue@1: }//QR
xue@1: 
Chris@5: /**
xue@1:   function QR_GS: QR decomposition A=QR using Gram-Schmidt method
xue@1: 
xue@1:   In: matrix A[M][N], M>=N
xue@1:   Out: Q[M][N], R[N][N]
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void QR_GS(int M, int N, double** A, double** Q, double** R)
xue@1: {
xue@1:   double *u=new double[M];
xue@1:   for (int n=0; n<N; n++)
xue@1:   {
xue@1:     memset(R[n], 0, sizeof(double)*N);
xue@1:     for (int m=0; m<M; m++) u[m]=A[m][n];
xue@1:     for (int k=0; k<n; k++)
xue@1:     {
xue@1:       double ip=0; for (int m=0; m<M; m++) ip+=u[m]*Q[m][k];
xue@1:       for (int m=0; m<M; m++) u[m]-=ip*Q[m][k];
xue@1:       R[k][n]=ip;
xue@1:     }
xue@1:     double iu=0; for (int m=0; m<M; m++) iu+=u[m]*u[m]; iu=sqrt(iu);
xue@1:     R[n][n]=iu;
xue@1:     iu=1.0/iu; for (int m=0; m<M; m++) Q[m][n]=u[m]*iu;
xue@1:   }
xue@1:   delete[] u;
xue@1: }//QR_GS
xue@1: 
Chris@5: /**
xue@1:   function QR_householder: QR decomposition using householder transform
xue@1: 
xue@1:   In: A[M][N], M>=N
xue@1:   Out: Q[M][M], R[M][N]
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void QR_householder(int M, int N, double** A, double** Q, double** R)
xue@1: {
xue@1:   double *u=new double[M*3], *ur=&u[M], *qu=&u[M*2];
xue@1:   for (int m=0; m<M; m++)
xue@1:   {
xue@1:     memcpy(R[m], A[m], sizeof(double)*N);
xue@1:     memset(Q[m], 0, sizeof(double)*M); Q[m][m]=1;
xue@1:   }
xue@1:   for (int n=0; n<N; n++)
xue@1:   {
xue@1:     double alf=0; for (int m=n; m<M; m++) alf+=R[m][n]*R[m][n]; alf=sqrt(alf);
xue@1:     if (R[n][n]>0) alf=-alf;
xue@1:     for (int m=n; m<M; m++) u[m]=R[m][n]; u[n]=u[n]-alf;
xue@1:     double iu2=0; for (int m=n; m<M; m++) iu2+=u[m]*u[m]; iu2=2.0/iu2;
xue@1:     for (int m=n; m<N; m++)
xue@1:     {
xue@1:       ur[m]=0; for (int k=n; k<M; k++) ur[m]+=u[k]*R[k][m];
xue@1:     }
xue@1:     for (int m=0; m<M; m++)
xue@1:     {
xue@1:       qu[m]=0; for (int k=n; k<M; k++) qu[m]+=Q[m][k]*u[k];
xue@1:     }
xue@1:     for (int m=n; m<M; m++) u[m]=u[m]*iu2;
xue@1:     for (int m=n; m<M; m++) for (int k=n; k<N; k++) R[m][k]-=u[m]*ur[k];
xue@1:     for (int m=0; m<M; m++) for (int k=n; k<M; k++) Q[m][k]-=qu[m]*u[k];
xue@1:   }
xue@1:   delete[] u;
xue@1: }//QR_householder
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function QU: Unitary decomposition A=QU, where Q is unitary and U is upper triangular
xue@1: 
xue@1:   In: matrix A[N][N]
xue@1:   Out: matrices Q[N][N], A[n][n] now containing U
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void QU(int N, double** Q, double** A)
xue@1: {
xue@1:   int sizeN=sizeof(double)*N;
xue@1:   for (int i=0; i<N; i++) {memset(Q[i], 0, sizeN); Q[i][i]=1;}
xue@1: 
xue@1:   double m, s, c, *tmpi=new double[N], *tmpj=new double[N];
xue@1:   for (int i=1; i<N; i++) for (int j=0; j<i; j++)
xue@1:     if (A[i][j]!=0)
xue@1:     {
xue@1:       m=sqrt(A[j][j]*A[j][j]+A[i][j]*A[i][j]);
xue@1:       s=A[i][j]/m;
xue@1:       c=A[j][j]/m;
xue@1:       for (int k=0; k<N; k++) tmpi[k]=-s*A[j][k]+c*A[i][k], tmpj[k]=c*A[j][k]+s*A[i][k];
xue@1:       memcpy(A[i], tmpi, sizeN), memcpy(A[j], tmpj, sizeN);
xue@1:       for (int k=0; k<N; k++) tmpi[k]=-s*Q[j][k]+c*Q[i][k], tmpj[k]=c*Q[j][k]+s*Q[i][k];
xue@1:       memcpy(Q[i], tmpi, sizeN), memcpy(Q[j], tmpj, sizeN);
xue@1:     }
xue@1:   delete[] tmpi; delete[] tmpj;
xue@1:   transpose(N, Q);
xue@1: }//QU
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function Real: extracts the real part of matrix X
xue@1: 
xue@1:   In: matrix x[M][N];
xue@1:   Out: matrix z[M][N]
xue@1: 
xue@1:   Returns pointer to z. z is created anew if z=0 is specified on start.
xue@1: */
xue@1: double** Real(int M, int N, double** z, cdouble** x, MList* List)
xue@1: {
xue@1:   if (!z){Allocate2(double, M, N, z); if (List) List->Add(z, 2);}
xue@1:   for (int m=0; m<M; m++) for (int n=0; n<N; n++) z[m][n]=x[m][n].x;
xue@1:   return z;
xue@1: }//Real
xue@1: double** Real(int M, int N, cdouble** x, MList* List){return Real(M, N, 0, x, List);}
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function Roots: finds the roots of a polynomial. x^N+p[N-1]x^(N-1)+p[N-2]x^(N-2)...+p[0]
xue@1: 
xue@1:   In: vector p[N] of polynomial coefficients.
xue@1:   Out: vector r[N] of roots.
xue@1: 
xue@1:   Returns 0 if successful.
xue@1: */
xue@1: int Roots(int N, double* p, cdouble* r)
xue@1: {
xue@1:   double** A=new double*[N]; A[0]=new double[N*N]; for (int i=1; i<N; i++) A[i]=&A[0][i*N];
xue@1:   for (int i=0; i<N; i++) A[0][i]=-p[N-1-i];
xue@1:   if (N>1) memset(A[1], 0, sizeof(double)*N*(N-1));
xue@1:   for (int i=1; i<N; i++) A[i][i-1]=1;
xue@1:   BalanceSim(N, A);
xue@1:   double result=QR(N, A, r);
xue@1:   delete[] A[0]; delete[] A;
xue@1:   return result;
xue@1: }//Roots
xue@1: //real implementation
xue@1: int Roots(int N, double* p, double* rr, double* ri)
xue@1: {
xue@1:   cdouble* r=new cdouble[N];
xue@1:   int result=Roots(N, p, r);
xue@1:   for (int n=0; n<N; n++) rr[n]=r[n].x, ri[n]=r[n].y;
xue@1:   delete[] r;
xue@1:   return result;
xue@1: }//Roots
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function SorI: Sor iteration algorithm for solving linear system Ax=b.
xue@1: 
xue@1:   Sor method is an extension of the Gaussian-Siedel method, with the latter equivalent to the former
xue@1:   with w set to 1. The Sor iteration is given by x(k)=(D-wL)^(-1)(((1-w)D+wU)x(k-1)+wb), where 0<w<2, D
xue@1:   is diagonal, L is lower triangular, U is upper triangular and A=L+D+U. Sor method converges if A is
xue@1:   positive definite.
xue@1: 
xue@1:   In: matrix A[N][N], vector b[N], initial vector x0[N]
xue@1:   Out: vector x0[N]
xue@1: 
xue@1:   Returns 0 if successful. Contents of matrix A and vector b are unchanged on return.
xue@1: */
xue@1: int SorI(int N, double* x0, double** a, double* b, double w, double ep, int maxiter)
xue@1: {
xue@1:   double e, v=1-w, *x=new double[N];
xue@1:   int k=0, sizeN=sizeof(double)*N;
xue@1:   while (k<maxiter)
xue@1:   {
xue@1:     for (int i=0; i<N; i++)
xue@1:     {
xue@1:       x[i]=b[i];
xue@1:       for (int j=0; j<i; j++) x[i]-=a[i][j]*x[j];
xue@1:       for (int j=i+1; j<N; j++) x[i]-=a[i][j]*x0[j];
xue@1:       x[i]=v*x0[i]+w*x[i]/a[i][i];
xue@1:     }
xue@1:     e=0; for (int j=0; j<N; j++) e+=fabs(x[j]-x0[j]);
xue@1:     memcpy(x0, x, sizeN);
xue@1:     if (e<ep) break;
xue@1:     k++;
xue@1:   }
xue@1:   delete[] x;
xue@1:   if (k>=maxiter) return 1;
xue@1:   return 0;
xue@1: }//SorI
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: //Submatrix routines
xue@1: 
Chris@5: /**
xue@1:   function SetSubMatrix: copy matrix x[Y][X] into matrix z at (Y1, X1).
xue@1: 
xue@1:   In: matrix x[Y][X], matrix z with dimensions no less than [Y+Y1][X+X1]
xue@1:   Out: matrix z, updated.
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void SetSubMatrix(double** z, double** x, int Y1, int Y, int X1, int X)
xue@1: {
xue@1:   for (int y=0; y<Y; y++) memcpy(&z[Y1+y][X1], x[y], sizeof(double)*X);
xue@1: }//SetSubMatrix
xue@1: //complex version
xue@1: void SetSubMatrix(cdouble** z, cdouble** x, int Y1, int Y, int X1, int X)
xue@1: {
xue@1:   for (int y=0; y<Y; y++) memcpy(&z[Y1+y][X1], x[y], sizeof(cdouble)*X);
xue@1: }//SetSubMatrix
xue@1: 
Chris@5: /**
xue@1:   function SubMatrix: extract a submatrix of x at (Y1, X1) to z[Y][X].
xue@1: 
xue@1:   In: matrix x of dimensions no less than [Y+Y1][X+X1]
xue@1:   Out: matrix z[Y][X].
xue@1: 
xue@1:   Returns pointer to z. z is created anew if z=0 is specifid on start.
xue@1: */
xue@1: cdouble** SubMatrix(cdouble** z, cdouble** x, int Y1, int Y, int X1, int X, MList* List)
xue@1: {
xue@1:   if (!z) {Allocate2(cdouble, Y, X, z); if (List) List->Add(z, 2);}
xue@1:   for (int y=0; y<Y; y++) memcpy(z[y], &x[Y1+y][X1], sizeof(cdouble)*X);
xue@1:   return z;
xue@1: }//SetSubMatrix
xue@1: //wrapper function
xue@1: cdouble** SubMatrix(cdouble** x, int Y1, int Y, int X1, int X, MList* List)
xue@1: {
xue@1:   return SubMatrix(0, x, Y1, Y, X1, X, List);
xue@1: }//SetSubMatrix
xue@1: 
Chris@5: /**
xue@1:   function SubVector: extract a subvector of x at X1 to z[X].
xue@1: 
xue@1:   In: vector x no shorter than X+X1.
xue@1:   Out: vector z[X].
xue@1: 
xue@1:   Returns pointer to z. z is created anew if z=0 is specifid on start.
xue@1: */
xue@1: cdouble* SubVector(cdouble* z, cdouble* x, int X1, int X, MList* List)
xue@1: {
xue@1:   if (!z){z=new cdouble[X]; if (List) List->Add(z, 1);}
xue@1:   memcpy(z, &x[X1], sizeof(cdouble)*X);
xue@1:   return z;
xue@1: }//SubVector
xue@1: //wrapper function
xue@1: cdouble* SubVector(cdouble* x, int X1, int X, MList* List)
xue@1: {
xue@1:   return SubVector(0, x, X1, X, List);
xue@1: }//SubVector
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function transpose: matrix transpose: A'->A
xue@1: 
xue@1:   In: matrix a[N][N]
xue@1:   Out: matrix a[N][N] after transpose
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void transpose(int N, double** a)
xue@1: {
xue@1:   double tmp;
xue@1:   for (int i=1; i<N; i++) for (int j=0; j<i; j++) {tmp=a[i][j]; a[i][j]=a[j][i]; a[j][i]=tmp;}
xue@1: }//transpose
xue@1: //complex version
xue@1: void transpose(int N, cdouble** a)
xue@1: {
xue@1:   cdouble tmp;
xue@1:   for (int i=1; i<N; i++) for (int j=0; j<i; j++) {tmp=a[i][j]; a[i][j]=a[j][i]; a[j][i]=tmp;}
xue@1: }//transpose
xue@1: 
Chris@5: /**
xue@1:   function transpose: matrix transpose: A'->Z
xue@1: 
xue@1:   In: matrix a[M][N]
xue@1:   Out: matrix z[N][M]
xue@1: 
xue@1:   Returns pointer to z. z is created anew if z=0 is specifid on start.
xue@1: */
xue@1: double** transpose(int N, int M, double** ta, double** a, MList* List)
xue@1: {
xue@1:   if (!ta) {Allocate2(double, N, M, ta);  if (List) List->Add(ta, 2);}
xue@1:   for (int n=0; n<N; n++) for (int m=0; m<M; m++) ta[n][m]=a[m][n];
xue@1:   return ta;
xue@1: }//transpose
xue@1: //wrapper function
xue@1: double** transpose(int N, int M, double** a, MList* List)
xue@1: {
xue@1:   return transpose(N, M, 0, a, List);
xue@1: }//transpose
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function Unitary: given x & y s.t. |x|=|y|, find unitary matrix P s.t. Px=y. P is given in closed form
xue@1:   as I-(x-y)(x-y)'/(x-y)'x
xue@1: 
xue@1:   In: vectors x[N] and y[N]
xue@1:   Out: matrix P[N][N]
xue@1: 
xue@1:   Returns pointer to P. P is created anew if P=0 is specified on start.
xue@1: */
xue@1: double** Unitary(int N, double** P, double* x, double* y, MList* List)
xue@1: {
xue@1:   if (!P) {Allocate2(double, N, N, P); if (List) List->Add(P, 2);}
xue@1:   int sizeN=sizeof(double)*N;
xue@1:   for (int i=0; i<N; i++) {memset(P[i], 0, sizeN); P[i][i]=1;}
xue@1: 
xue@1:   double* w=MultiAdd(N, x, y, -1.0); //w=x-y
xue@1:   double m=Inner(N, x, w); //m=(x-y)'x
xue@1:   if (m!=0)
xue@1:   {
xue@1:     m=1.0/m; //m=1/(x-y)'x
xue@1:     double* mw=Multiply(N, w, m);
xue@1:     for (int i=0; i<N; i++) for (int j=0; j<N; j++) P[i][j]=P[i][j]-mw[i]*w[j];
xue@1:     delete[] mw;
xue@1:   }
xue@1:   delete[] w;
xue@1:   return P;
xue@1: }//Unitary
xue@1: //complex version
xue@1: cdouble** Unitary(int N, cdouble** P, cdouble* x, cdouble* y, MList* List)
xue@1: {
xue@1:   if (!P) {Allocate2(cdouble, N, N, P);}
xue@1:   int sizeN=sizeof(cdouble)*N;
xue@1:   for (int i=0; i<N; i++) {memset(P[i], 0, sizeN); P[i][i]=1;}
xue@1: 
xue@1:   cdouble *w=MultiAdd(N, x, y, -1);
xue@1:   cdouble m=Inner(N, x, w);
xue@1:   if (m!=0)
xue@1:   {
xue@1:     m=m.cinv();
xue@1:     cdouble *mw=Multiply(N, w, m);
xue@1:     for (int i=0; i<N; i++) for (int j=0; j<N; j++) P[i][j]=P[i][j]-(mw[i]^w[j]),
xue@1:     delete[] mw;
xue@1:   }
xue@1:   delete[] w;
xue@1:   if (List) List->Add(P, 2);
xue@1:   return P;
xue@1: }//Unitary
xue@1: //wrapper functions
xue@1: double** Unitary(int N, double* x, double* y, MList* List){return Unitary(N, 0, x, y, List);}
xue@1: cdouble** Unitary(int N, cdouble* x, cdouble* y, MList* List){return Unitary(N, 0, x, y, List);}
xue@1: 
xue@1: 
xue@1: