/* dgeco.c -- double precision solution of general matrix by * Gaussian elimination with condition estimate. * * From "Linpack Users' Guide", Dongarra, et al., */ #include #include #include "func.h" #define max(a,b) (((a) > (b)) ? (a) : (b)) #define min(a,b) (((a) < (b)) ? (a) : (b)) #define signum(a,b) (((b) < (0)) ? (-a) : (a)) void dgeco(double **a,int n, int *ipvt, double *rcond,double *z) { double anorm,ek,s,sm,t,vecdot,vecsum,wk,wkm,ynorm; int i,info,j,k,kb,kp1,l; /* Compute 1-norm of a */ anorm = 0.0; for (j = 0; j < n; j++) { vecsum = 0.0; for (i = 0;i < n; i++) vecsum += fabs(a[i][j]); anorm = max(anorm,vecsum); } /* Factor. */ dgefa(a,n,ipvt,&info); /* rcond = 1/(norm(a) * (estimate of norm(inverse(a)))). * estimate = norm(z)/norm(y), where a*z=y and trans(a)*y=e. * trans(a) is the transpose of a. The components of e are * chosen to cause maximum local growth in the elements of * w, where trans(u)*w=e. The vectors are frequently rescaled * to avoid overflow. */ ek = 1.0; for (j = 0; j < n; j++) z[j] = 0.0; for (k = 0; k < n; k++) { if (z[k] != 0.0) ek = signum(ek,-z[k]); if (fabs(ek-z[k]) > fabs(a[k][k])) { s = fabs(a[k][k])/fabs(ek-z[k]); /* dscal(n,s,z,1) */ for (i = 0; i < n; i++) z[i] *= s; ek *= s; } wk = ek - z[k]; wkm = -ek - z[k]; s = fabs(wk); sm = fabs(wkm); if (a[k][k] != 0.0) { wk /= a[k][k]; wkm /= a[k][k]; } else { wk = 1.0; wkm = 1.0; } kp1 = k + 1; if (kp1 < n) { for (j = kp1; j < n; j++) { sm += (fabs(z[j] + wkm * a[k][j])); z[j] += (wk * a[k][j]); s += fabs(z[j]); } if (s < sm) { t = wkm -wk; wk = wkm; for (j = kp1; j < n; j++) z[j] += (t * a[k][j]); } } z[k] = wk; } /* dasum(n,s,z,1) */ vecsum = 0.0; for (i = 0;i < n; i++) vecsum += fabs(z[i]); s = 1.0/vecsum; /* dscal(n,s,z) */ for (i = 0; i < n; i++) z[i] *= s; /* Solve trans(l)*y= w */ for (kb = 0; kb < n; kb++) { k = n - kb - 1; if (k < (n-1)) { /* sdot(n-k,a(k+1,k),1,z(k+1),1) */ vecdot = 0.0; for (i = k+1;i < n; i++) vecdot += (a[i][k] * z[i]); z[k] += vecdot; } if (fabs(z[k]) > 1.0) { s = 1.0/fabs(z[k]); /* dscal(n,s,z) */ for (i = 0; i < n; i++) z[i] *= s; } l = ipvt[k]; t = z[l]; z[l] = z[k]; z[k] = t; } /* endfor kb */ /* dasum(n,z,1) */ vecsum = 0.0; for (i = 0; i < n; i++) vecsum += fabs(z[i]); s = 1.0/vecsum; /* dscal(n,s,z) */ for (i = 0; i < n; i++) z[i] *= s; ynorm = 1.0; /* * Solve l * v = y */ for (k = 0; k < n; k++) { l = ipvt[k]; t = z[l]; z[l] = z[k]; z[k] = t; if (k < (n-1)) { /* daxpy(n-k,t,a[k+1][k],1,z[k+1],1) */ for (i = k+1;i < n; i++) z[i] += (t * a[i][k]); } if (fabs(z[k]) > 1.0) { s = 1.0/fabs(z[k]); /* dscal(n,s,z,1) */ for (i = 0; i < n; i++) z[i] *= s; ynorm *= s; } } /* dasum(n,z,1) */ vecsum = 0.0; for (i = 0; i < n; i++) vecsum += fabs(z[i]); s = 1.0/vecsum; /* dscal(n,s,z,1) */ for (i = 0; i < n; i++) z[i] *= s; ynorm *= s; /* Solve u * z = v */ for (kb = 0; kb < n; kb++) { k = n - kb - 1; if (fabs(z[k]) > fabs(a[k][k])) { s = fabs(a[k][k])/fabs(z[k]); /* dscal(n,s,z,1) */ for (i = 0; i < n; i++) z[i] *= s; ynorm *= s; } if (a[k][k] != 0.0) z[k] /= a[k][k]; if (a[k][k] == 0.0) z[k] = 1.0; t = -z[k]; /* daxpy(k-1,t,a[1][k],1,z[1],1) */ for (i = 0; i < k; i++) z[i] += (t * a[i][k]); } /* Make znorm = 1.0 */ /* dasum(n,z,1) */ vecsum = 0.0; for (i = 0; i < n; i++) vecsum += fabs(z[i]); s = 1.0/vecsum; /* dscal(n,s,z,1) */ for (i = 0; i < n; i++) z[i] *= s; ynorm *= s; if (anorm != 0.0) *rcond = ynorm/anorm; if (anorm == 0.0) *rcond = 0.0; }