use doc'd parameter as per SBCL docs; clean up do-indentation; fix ffix macro to...
[CommonLispStat.git] / lib / rcondest.c
blob8361e3bbb12a8e94ab49a59d053cca65892e422f
1 /* rcondest - Estimates reciprocal of condition number. */
2 /* XLISP-STAT 2.1 Copyright (c) 1990, by Luke Tierney */
3 /* Additions to Xlisp 2.1, Copyright (c) 1989 by David Michael Betz */
4 /* You may give out copies of this software; for conditions see the */
5 /* file COPYING included with this distribution. */
7 #include "linalg.h"
9 static double Max(a, b)
10 double a, b;
12 return(a > b ? a : b);
15 double rcondest(a, n)
16 RMatrix a;
17 int n;
19 RVector p, pm, x;
20 double est, xp, xm, temp, tempm, xnorm;
21 int i, j;
23 for (i = 0; i < n; i++)
24 if (a[i][i] == 0.0) return(0.0);
26 p = rvector(n);
27 pm = rvector(n);
28 x = rvector(n);
30 /* Set est to reciprocal of L1 matrix norm of A */
31 est = fabs(a[0][0]);
32 for (j = 1; j < n; j++) {
33 for (i = 0, temp = fabs(a[j][j]); i < j; i++)
34 temp += fabs(a[i][j]);
35 est = Max(est, temp);
37 est = 1.0 / est;
39 /* Solve A^Tx = e, selecting e as you proceed */
40 x[0] = 1.0 / a[0][0];
41 for (i = 1; i < n; i++) p[i] = a[0][i] * x[0];
42 for (j = 1; j < n; j++) {
43 /* select ej and calculate x[j] */
44 xp = ( 1.0 - p[j]) / a[j][j];
45 xm = (-1.0 - p[j]) / a[j][j];
46 temp = fabs(xp);
47 tempm = fabs(xm);
48 for (i = j + 1; i < n; i++) {
49 pm[i] = p[i] + a[j][i] * xm;
50 tempm += fabs(pm[i] / a[i][i]);
51 p[i] += a[j][i] * xp;
52 temp += fabs(p[i] / a[i][i]);
54 if (temp >= tempm) x[j] = xp;
55 else {
56 x[j] = xm;
57 for (i = j + 1; i < n; i++) p[i] = pm[i];
61 for (j = 0, xnorm = 0.0; j < n; j++) xnorm += fabs(x[j]);
62 est = est * xnorm;
63 backsolve(a, x, n, RE);
64 for (j = 0, xnorm = 0.0; j < n; j++) xnorm += fabs(x[j]);
65 if (xnorm > 0) est = est / xnorm;
67 free_vector(p);
68 free_vector(pm);
69 free_vector(x);
71 return(est);
74 backsolve(a, b, n, mode)
75 Matrix a;
76 Vector b;
77 int n;
79 int i, j;
80 RMatrix ra = (RMatrix) a;
81 RVector rb = (RVector) b;
82 CMatrix ca = (CMatrix) a;
83 CVector cb = (CVector) b;
85 switch (mode) {
86 case RE:
87 for (i = n - 1; i >= 0; i--) {
88 if (ra[i][i] != 0.0) rb[i] = rb[i] / ra[i][i];
89 for (j = i + 1; j < n; j++) rb[i] -= ra[i][j] * rb[j];
91 break;
92 case CX:
93 for (i = n - 1; i >= 0; i--) {
94 if (modulus(ca[i][i]) != 0.0) cb[i] = cdiv(cb[i], ca[i][i]);
95 for (j = i + 1; j < n; j++)
96 cb[i] = csub(cb[i], cmul(ca[i][j], cb[j]));
98 break;