Merge branch 'new-jacobi' into improve-include-consistency

src/math_extra.cpp

@@ -13,6 +13,7 @@
 
 /* ----------------------------------------------------------------------
    Contributing author: Mike Brown (SNL)
+                        Arno Mayrhofer (DCS Computing), jacobi() functions
 ------------------------------------------------------------------------- */
 
 #include "math_extra.h"
@@ -95,83 +96,189 @@ int mldivide3(const double m[3][3], const double *v, double *ans)
 /* ----------------------------------------------------------------------
    compute evalues and evectors of 3x3 real symmetric matrix
    based on Jacobi rotations
-   adapted from Numerical Recipes jacobi() function
+   two variants for passing in matrix
+
+   procedure jacobi(S ∈ Rn×n; out e ∈ Rn; out E ∈ Rn×n)
+     var
+       i, k, l, m, state ∈ N
+       s, c, t, p, y, d, r ∈ R
+       ind ∈ Nn
+       changed ∈ Ln
+     ! init e, E, and arrays ind, changed
+     E := I; state := n
+     for k := 1 to n do indk := maxind(k); ek := Skk; changedk := true endfor
+     while state≠0 do ! next rotation
+       m := 1 ! find index (k,l) of pivot p
+       for k := 2 to n−1 do
+         if │Sk indk│ > │Sm indm│ then m := k endif
+       endfor
+       k := m; l := indm; p := Skl
+       ! calculate c = cos φ, s = sin φ
+       y := (el−ek)/2; d := │y│+√(p2+y2)
+       r := √(p2+d2); c := d/r; s := p/r; t := p2/d
+       if y<0 then s := −s; t := −t endif
+       Skl := 0.0; update(k,−t); update(l,t)
+       ! rotate rows and columns k and l
+       for i := 1 to k−1 do rotate(i,k,i,l) endfor
+       for i := k+1 to l−1 do rotate(k,i,i,l) endfor
+       for i := l+1 to n do rotate(k,i,l,i) endfor
+       ! rotate eigenvectors
+       for i := 1 to n do
+         ┌   ┐    ┌     ┐┌   ┐
+         │Eik│    │c  −s││Eik│
+         │   │ := │     ││   │
+         │Eil│    │s   c││Eil│
+         └   ┘    └     ┘└   ┘
+       endfor
+       ! rows k, l have changed, update rows indk, indl
+       indk := maxind(k); indl := maxind(l)
+     loop
+   endproc
 ------------------------------------------------------------------------- */
 
 int jacobi(double matrix[3][3], double *evalues, double evectors[3][3])
 {
-  int i,j,k;
+  evectors[0][0] = 1.0; evectors[0][1] = 0.0; evectors[0][2] = 0.0;
-  double tresh,theta,tau,t,sm,s,h,g,c,b[3],z[3];
+  evectors[1][0] = 0.0; evectors[1][1] = 1.0; evectors[1][2] = 0.0;
+  evectors[2][0] = 0.0; evectors[2][1] = 0.0; evectors[2][2] = 1.0;
+  evalues[0] = 0.0; evalues[1] = 0.0; evalues[2] = 0.0;
+  double threshold = 0.0;
 
-  for (i = 0; i < 3; i++) {
+  for (int i = 0; i < 3; i++)
-    for (j = 0; j < 3; j++) evectors[i][j] = 0.0;
+    for (int j = i; j < 3; j++)
-    evectors[i][i] = 1.0;
+      threshold += fabs(matrix[i][j]);
-  }
-  for (i = 0; i < 3; i++) {
-    b[i] = evalues[i] = matrix[i][i];
-    z[i] = 0.0;
-  }
 
-  for (int iter = 1; iter <= MAXJACOBI; iter++) {
+  if (threshold < 1.0e-200) return 0;
-    sm = 0.0;
+  threshold *= 1.0e-12;
-    for (i = 0; i < 2; i++)
+  int state = 2;
-      for (j = i+1; j < 3; j++)
+  bool changed[3] = {true, true, true};
-        sm += fabs(matrix[i][j]);
-    if (sm == 0.0) return 0;
 
-    if (iter < 4) tresh = 0.2*sm/(3*3);
+  int iteration = 0;
-    else tresh = 0.0;
+  while (state > 0 && iteration < MAXJACOBI) {
-
+    for (int k = 0; k < 2; k++) {
-    for (i = 0; i < 2; i++) {
+      for (int l = k+1; l < 3; l++) {
-      for (j = i+1; j < 3; j++) {
+        const double p = matrix[k][l];
-        g = 100.0*fabs(matrix[i][j]);
+        const double y = (matrix[l][l]-matrix[k][k])*0.5;
-        if (iter > 4 && fabs(evalues[i])+g == fabs(evalues[i])
+        const double d = fabs(y)+sqrt(p*p + y*y);
-            && fabs(evalues[j])+g == fabs(evalues[j]))
+        const double r = sqrt(p*p + d*d);
-          matrix[i][j] = 0.0;
+        const double c = r > threshold ? d/r : 1.0;
-        else if (fabs(matrix[i][j]) > tresh) {
+        double s = r > threshold ? p/r : 0.0;
-          h = evalues[j]-evalues[i];
+        double t = d > threshold ? p*p/d : 0.0;
-          if (fabs(h)+g == fabs(h)) t = (matrix[i][j])/h;
+        if (y < 0.0) {
-          else {
+          s *= -1.0;
-            theta = 0.5*h/(matrix[i][j]);
+          t *= -1.0;
-            t = 1.0/(fabs(theta)+sqrt(1.0+theta*theta));
-            if (theta < 0.0) t = -t;
-          }
-          c = 1.0/sqrt(1.0+t*t);
-          s = t*c;
-          tau = s/(1.0+c);
-          h = t*matrix[i][j];
-          z[i] -= h;
-          z[j] += h;
-          evalues[i] -= h;
-          evalues[j] += h;
-          matrix[i][j] = 0.0;
-          for (k = 0; k < i; k++) rotate(matrix,k,i,k,j,s,tau);
-          for (k = i+1; k < j; k++) rotate(matrix,i,k,k,j,s,tau);
-          for (k = j+1; k < 3; k++) rotate(matrix,i,k,j,k,s,tau);
-          for (k = 0; k < 3; k++) rotate(evectors,k,i,k,j,s,tau);
         }
+        matrix[k][l] = 0.0;
+        update_eigenvalue(matrix[k][k], changed[k], state, -t, threshold);
+        update_eigenvalue(matrix[l][l], changed[l], state,  t, threshold);
+        for (int i = 0; i < k; i++)
+          rotate(matrix[i][k], matrix[i][l],c,s);
+        for (int i = k+1; i < l; i++)
+          rotate(matrix[k][i], matrix[i][l],c,s);
+        for (int i = l+1; i < 3; i++)
+          rotate(matrix[k][i], matrix[l][i],c,s);
+        for (int i = 0; i < 3; i++)
+          rotate(evectors[i][k], evectors[i][l],c,s);
       }
     }
-
+    iteration++;
-    for (i = 0; i < 3; i++) {
-      evalues[i] = b[i] += z[i];
-      z[i] = 0.0;
-    }
   }
-  return 1;
+
+  for (int i = 0; i < 3; i++)
+    evalues[i] = matrix[i][i];
+
+  if (iteration == MAXJACOBI) return 1;
+  return 0;
+}
+
+int jacobi(double **matrix, double *evalues, double **evectors)
+{
+  evectors[0][0] = 1.0; evectors[0][1] = 0.0; evectors[0][2] = 0.0;
+  evectors[1][0] = 0.0; evectors[1][1] = 1.0; evectors[1][2] = 0.0;
+  evectors[2][0] = 0.0; evectors[2][1] = 0.0; evectors[2][2] = 1.0;
+  evalues[0] = 0.0; evalues[1] = 0.0; evalues[2] = 0.0;
+  double threshold = 0.0;
+
+  for (int i = 0; i < 3; i++)
+    for (int j = i; j < 3; j++)
+      threshold += fabs(matrix[i][j]);
+
+  if (threshold < 1.0e-200) return 0;
+  threshold *= 1.0e-12;
+  int state = 2;
+  bool changed[3] = {true, true, true};
+
+  int iteration = 0;
+  while (state > 0 && iteration < MAXJACOBI) {
+    for (int k = 0; k < 2; k++) {
+      for (int l = k+1; l < 3; l++) {
+        const double p = matrix[k][l];
+        const double y = (matrix[l][l]-matrix[k][k])*0.5;
+        const double d = fabs(y)+sqrt(p*p + y*y);
+        const double r = sqrt(p*p + d*d);
+        const double c = r > threshold ? d/r : 1.0;
+        double s = r > threshold ? p/r : 0.0;
+        double t = d > threshold ? p*p/d : 0.0;
+        if (y < 0.0) {
+          s *= -1.0;
+          t *= -1.0;
+        }
+        matrix[k][l] = 0.0;
+        update_eigenvalue(matrix[k][k], changed[k], state, -t, threshold);
+        update_eigenvalue(matrix[l][l], changed[l], state,  t, threshold);
+        for (int i = 0; i < k; i++)
+          rotate(matrix[i][k], matrix[i][l],c,s);
+        for (int i = k+1; i < l; i++)
+          rotate(matrix[k][i], matrix[i][l],c,s);
+        for (int i = l+1; i < 3; i++)
+          rotate(matrix[k][i], matrix[l][i],c,s);
+        for (int i = 0; i < 3; i++)
+          rotate(evectors[i][k], evectors[i][l],c,s);
+      }
+    }
+    iteration++;
+  }
+
+  for (int i = 0; i < 3; i++)
+    evalues[i] = matrix[i][i];
+
+  if (iteration == MAXJACOBI) return 1;
+  return 0;
 }
 
 /* ----------------------------------------------------------------------
-   perform a single Jacobi rotation
+   perform a single Jacobi rotation of Sij, Skl
+    ┌   ┐    ┌     ┐┌   ┐
+    │Skl│    │c  −s││Skl│
+    │   │ := │     ││   │
+    │Sij│    │s   c││Sij│
+    └   ┘    └     ┘└   ┘
 ------------------------------------------------------------------------- */
 
-void rotate(double matrix[3][3], int i, int j, int k, int l,
+void rotate(double &matrix_kl, double &matrix_ij, 
-            double s, double tau)
+            const double c, const double s)
 {
-  double g = matrix[i][j];
+  const double tmp_kl = matrix_kl;
-  double h = matrix[k][l];
+  matrix_kl = c*matrix_kl - s*matrix_ij;
-  matrix[i][j] = g-s*(h+g*tau);
+  matrix_ij = s*tmp_kl    + c*matrix_ij;
-  matrix[k][l] = h+s*(g-h*tau);
+}
+
+/* ----------------------------------------------------------------------
+   update eigenvalue and its status
+------------------------------------------------------------------------- */
+
+void update_eigenvalue(double &eigenvalue, bool &changed, int &state, 
+                       const double t, const double threshold)
+{
+  eigenvalue += t;
+
+  if (changed && fabs(t) < threshold) {
+    changed = false;
+    state--;
+  } else if (!changed && fabs(t) > threshold) {
+    changed = true;
+    state++;
+  }
 }
 
 /* ----------------------------------------------------------------------

src/math_extra.h

@@ -74,9 +74,14 @@ namespace MathExtra {
 
   void write3(const double mat[3][3]);
   int mldivide3(const double mat[3][3], const double *vec, double *ans);
+
   int jacobi(double matrix[3][3], double *evalues, double evectors[3][3]);
-  void rotate(double matrix[3][3], int i, int j, int k, int l,
+  int jacobi(double **matrix, double *evalues, double **evectors);
-              double s, double tau);
+  void rotate(double &matrix_kl, double &matrix_ij, 
+              const double c, const double s);
+  void update_eigenvalue(double &eigenvalue, bool &changed, int &state, 
+                         const double t, const double threshold);
+
   void richardson(double *q, double *m, double *w, double *moments, double dtq);
   void no_squish_rotate(int k, double *p, double *q, double *inertia,
                         double dt);