c_cpp/lib/klib/keigen.c

   1 #include <math.h>
   2 #include <stdlib.h>
   3 #include "keigen.h"
   4
   5 void ke_core_strq(int n, double *q, double *b, double *c)
   6 {
   7         int     i, j, k, u, v;
   8         double h, f, g, h2;
   9         for (i = n - 1; i >= 1; i--) {
  10                 h = 0.0;
  11                 if (i > 1)
  12                         for (k = 0; k < i; k++) {
  13                                 u = i * n + k;
  14                                 h = h + q[u] * q[u];
  15                         }
  16                 if (h + 1.0 == 1.0) {
  17                         c[i] = 0.0;
  18                         if (i == 1)
  19                                 c[i] = q[i * n + i - 1];
  20                         b[i] = 0.0;
  21                 } else {
  22                         c[i] = sqrt(h);
  23                         u = i * n + i - 1;
  24                         if (q[u] > 0.0)
  25                                 c[i] = -c[i];
  26                         h = h - q[u] * c[i];
  27                         q[u] = q[u] - c[i];
  28                         f = 0.0;
  29                         for (j = 0; j < i; j++) {
  30                                 q[j * n + i] = q[i * n + j] / h;
  31                                 g = 0.0;
  32                                 for (k = 0; k <= j; k++)
  33                                         g = g + q[j * n + k] * q[i * n + k];
  34                                 if (j + 1 < i)
  35                                         for (k = j + 1; k <= i - 1; k++)
  36                                                 g = g + q[k * n + j] * q[i * n + k];
  37                                 c[j] = g / h;
  38                                 f = f + g * q[j * n + i];
  39                         }
  40                         h2 = f / (h + h);
  41                         for (j = 0; j < i; j++) {
  42                                 f = q[i * n + j];
  43                                 g = c[j] - h2 * f;
  44                                 c[j] = g;
  45                                 for (k = 0; k <= j; k++) {
  46                                         u = j * n + k;
  47                                         q[u] = q[u] - f * c[k] - g * q[i * n + k];
  48                                 }
  49                         }
  50                         b[i] = h;
  51                 }
  52         }
  53         for (i = 0; i < n - 1; i++)
  54                 c[i] = c[i + 1];
  55         c[n - 1] = 0.0;
  56         b[0] = 0.0;
  57         for (i = 0; i < n; i++) {
  58                 if (b[i] != 0.0 && i - 1 >= 0)
  59                         for (j = 0; j < i; j++) {
  60                                 g = 0.0;
  61                                 for (k = 0; k < i; k++)
  62                                         g = g + q[i * n + k] * q[k * n + j];
  63                                 for (k = 0; k < i; k++) {
  64                                         u = k * n + j;
  65                                         q[u] = q[u] - g * q[k * n + i];
  66                                 }
  67                         }
  68                 u = i * n + i;
  69                 b[i] = q[u];
  70                 q[u] = 1.0;
  71                 if (i - 1 >= 0)
  72                         for (j = 0; j < i; j++) {
  73                                 q[i * n + j] = 0.0;
  74                                 q[j * n + i] = 0.0;
  75                         }
  76         }
  77 }
  78
  79 int ke_core_sstq(int n, double *b, double *c, double *q, int cal_ev, double eps, int l)
  80 {
  81         int i, j, k, m, it, u, v;
  82         double d, f, h, g, p, r, e, s;
  83         c[n - 1] = 0.0;
  84         d = 0.0;
  85         f = 0.0;
  86         for (j = 0; j < n; j++) {
  87                 it = 0;
  88                 h = eps * (fabs(b[j]) + fabs(c[j]));
  89                 if (h > d)
  90                         d = h;
  91                 m = j;
  92                 while (m < n && fabs(c[m]) > d)
  93                         m = m + 1;
  94                 if (m != j) {
  95                         do {
  96                                 if (it == l) return KE_EXCESS_ITER;
  97                                 it = it + 1;
  98                                 g = b[j];
  99                                 p = (b[j + 1] - g) / (2.0 * c[j]);
 100                                 r = sqrt(p * p + 1.0);
 101                                 if (p >= 0.0)
 102                                         b[j] = c[j] / (p + r);
 103                                 else
 104                                         b[j] = c[j] / (p - r);
 105                                 h = g - b[j];
 106                                 for (i = j + 1; i < n; i++)
 107                                         b[i] = b[i] - h;
 108                                 f = f + h;
 109                                 p = b[m];
 110                                 e = 1.0;
 111                                 s = 0.0;
 112                                 for (i = m - 1; i >= j; i--) {
 113                                         g = e * c[i];
 114                                         h = e * p;
 115                                         if (fabs(p) >= fabs(c[i])) {
 116                                                 e = c[i] / p;
 117                                                 r = sqrt(e * e + 1.0);
 118                                                 c[i + 1] = s * p * r;
 119                                                 s = e / r;
 120                                                 e = 1.0 / r;
 121                                         } else {
 122                                                 e = p / c[i];
 123                                                 r = sqrt(e * e + 1.0);
 124                                                 c[i + 1] = s * c[i] * r;
 125                                                 s = 1.0 / r;
 126                                                 e = e / r;
 127                                         }
 128                                         p = e * b[i] - s * g;
 129                                         b[i + 1] = h + s * (e * g + s * b[i]);
 130                                         if (cal_ev) {
 131                                                 for (k = 0; k < n; k++) {
 132                                                         u = k * n + i + 1;
 133                                                         v = u - 1;
 134                                                         h = q[u];
 135                                                         q[u] = s * q[v] + e * h;
 136                                                         q[v] = e * q[v] - s * h;
 137                                                 }
 138                                         }
 139                                 }
 140                                 c[j] = s * p;
 141                                 b[j] = e * p;
 142                         }
 143                         while (fabs(c[j]) > d);
 144                 }
 145                 b[j] = b[j] + f;
 146         }
 147         for (i = 0; i < n; i++) {
 148                 k = i;
 149                 p = b[i];
 150                 if (i + 1 < n) {
 151                         j = i + 1;
 152                         while (j < n && b[j] <= p) {
 153                                 k = j;
 154                                 p = b[j];
 155                                 j = j + 1;
 156                         }
 157                 }
 158                 if (k != i) {
 159                         b[k] = b[i];
 160                         b[i] = p;
 161                         for (j = 0; j < n; j++) {
 162                                 u = j * n + i;
 163                                 v = j * n + k;
 164                                 p = q[u];
 165                                 q[u] = q[v];
 166                                 q[v] = p;
 167                         }
 168                 }
 169         }
 170         return 0;
 171 }
 172
 173 #define MALLOC(type, size) ((type*)malloc(size * sizeof(type)))
 174
 175 int ke_eigen_sd(int n, double *a, double *v, int cal_ev, double eps, int max_iter)
 176 {
 177         double *c;
 178         int r;
 179         if (1.0 + eps <= 1.0) eps = 1e-7;
 180         if (max_iter <= 0) max_iter = 50;
 181         c = MALLOC(double, n);
 182         ke_core_strq(n, a, v, c);
 183         r = ke_core_sstq(n, v, c, a, cal_ev, eps, max_iter);
 184         free(c);
 185         return r;
 186 }