nr_jacobi.C

00001 // -*- c++ -*-
00002 
00003 #include "colvartypes.h"
00004 #include "nr_jacobi.h"
00005 
00006 
00007 #define ROTATE(a,i,j,k,l) g=a[i][j]; \
00008 h=a[k][l]; \
00009 a[i][j]=g-s*(h+g*tau); \
00010 a[k][l]=h+s*(g-h*tau);
00011 
00012 #define n 4
00013 
00014 
00015 namespace NR_Jacobi {
00016 
00017 int jacobi(cvm::real **a, cvm::real *d, cvm::real **v, int *nrot)
00018 {
00019 int j,iq,ip,i;
00020 cvm::real tresh,theta,tau,t,sm,s,h,g,c;
00021 
00022 cvm::vector1d<cvm::real> b(n);
00023 cvm::vector1d<cvm::real> z(n);
00024 
00025 for (ip=0;ip<n;ip++) {
00026 for (iq=0;iq<n;iq++) {
00027 v[ip][iq]=0.0;
00028 }
00029 v[ip][ip]=1.0;
00030 }
00031 for (ip=0;ip<n;ip++) {
00032 b[ip]=d[ip]=a[ip][ip];
00033 z[ip]=0.0;
00034 }
00035 *nrot=0;
00036 for (i=0;i<=50;i++) {
00037 sm=0.0;
00038 for (ip=0;ip<n-1;ip++) {
00039 for (iq=ip+1;iq<n;iq++)
00040 sm += cvm::fabs(a[ip][iq]);
00041 }
00042 if (sm == 0.0) {
00043 return COLVARS_OK;
00044 }
00045 if (i < 4)
00046 tresh=0.2*sm/(n*n);
00047 else
00048 tresh=0.0;
00049 for (ip=0;ip<n-1;ip++) {
00050 for (iq=ip+1;iq<n;iq++) {
00051 g=100.0*cvm::fabs(a[ip][iq]);
00052 if (i > 4 && (cvm::real)(cvm::fabs(d[ip])+g) == (cvm::real)cvm::fabs(d[ip])
00053 && (cvm::real)(cvm::fabs(d[iq])+g) == (cvm::real)cvm::fabs(d[iq]))
00054 a[ip][iq]=0.0;
00055 else if (cvm::fabs(a[ip][iq]) > tresh) {
00056 h=d[iq]-d[ip];
00057 if ((cvm::real)(cvm::fabs(h)+g) == (cvm::real)cvm::fabs(h))
00058 t=(a[ip][iq])/h;
00059 else {
00060 theta=0.5*h/(a[ip][iq]);
00061 t=1.0/(cvm::fabs(theta)+cvm::sqrt(1.0+theta*theta));
00062 if (theta < 0.0) t = -t;
00063 }
00064 c=1.0/cvm::sqrt(1+t*t);
00065 s=t*c;
00066 tau=s/(1.0+c);
00067 h=t*a[ip][iq];
00068 z[ip] -= h;
00069 z[iq] += h;
00070 d[ip] -= h;
00071 d[iq] += h;
00072 a[ip][iq]=0.0;
00073 for (j=0;j<=ip-1;j++) {
00074 ROTATE(a,j,ip,j,iq)
00075 }
00076 for (j=ip+1;j<=iq-1;j++) {
00077 ROTATE(a,ip,j,j,iq)
00078 }
00079 for (j=iq+1;j<n;j++) {
00080 ROTATE(a,ip,j,iq,j)
00081 }
00082 for (j=0;j<n;j++) {
00083 ROTATE(v,j,ip,j,iq)
00084 }
00085 ++(*nrot);
00086 }
00087 }
00088 }
00089 for (ip=0;ip<n;ip++) {
00090 b[ip] += z[ip];
00091 d[ip]=b[ip];
00092 z[ip]=0.0;
00093 }
00094 }
00095 return COLVARS_ERROR;
00096 }
00097 
00098 
00099 int eigsrt(cvm::real *d, cvm::real **v)
00100 {
00101 int k,j,i;
00102 cvm::real p;
00103 
00104 for (i=0;i<n;i++) {
00105 p=d[k=i];
00106 for (j=i+1;j<n;j++)
00107 if (d[j] >= p) p=d[k=j];
00108 if (k != i) {
00109 d[k]=d[i];
00110 d[i]=p;
00111 for (j=0;j<n;j++) {
00112 p=v[j][i];
00113 v[j][i]=v[j][k];
00114 v[j][k]=p;
00115 }
00116 }
00117 }
00118 return COLVARS_OK;
00119 }
00120 
00121 
00122 int transpose(cvm::real **v)
00123 {
00124 cvm::real p;
00125 int i,j;
00126 for (i=0;i<n;i++) {
00127 for (j=i+1;j<n;j++) {
00128 p=v[i][j];
00129 v[i][j]=v[j][i];
00130 v[j][i]=p;
00131 }
00132 }
00133 return COLVARS_OK;
00134 }
00135 
00136 }
00137 
00138 #undef n
00139 #undef ROTATE