--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/toolbox/diagpiv.m	Thu May 07 18:36:24 2015 +0200
@@ -0,0 +1,108 @@
+function [L, D, P, rho] = diagpiv(A)
+%DIAGPIV     Diagonal pivoting factorization with partial pivoting.
+%            Given a Hermitian matrix A,
+%            [L, D, P, rho] = DIAGPIV(A) computes a permutation P,
+%            a unit lower triangular L, and a real block diagonal D
+%            with 1x1 and 2x2 diagonal blocks, such that
+%            P*A*P' = L*D*L'.
+%            The Bunch-Kaufman partial pivoting strategy is used.
+%            Rho is the growth factor.
+
+%            Reference:
+%            J.R. Bunch and L. Kaufman, Some stable methods for calculating
+%            inertia and solving symmetric linear systems, Math. Comp.,
+%            31(137):163-179, 1977.
+
+%            This routine does not exploit symmetry and is not designed to be
+%            efficient.
+
+if norm(triu(A,1)'-tril(A,-1),1), error('Must supply Hermitian matrix.'), end
+
+n = max(size(A));
+k = 1;
+D = eye(n);
+L = eye(n);
+pp = 1:n;
+normA = norm(A(:),inf);
+rho = normA;
+
+alpha = (1 + sqrt(17))/8;
+
+while k < n
+      [lambda, r] = max( abs(A(k+1:n,k)) );
+      r = r(1) + k;
+
+      if lambda > 0
+          swap = 0;
+          if abs(A(k,k)) >= alpha*lambda
+             s = 1;
+          else
+             temp = A(k:n,r); temp(r-k+1) = 0;
+             sigma = norm(temp, inf);
+             if alpha*lambda^2 <= abs(A(k,k))*sigma
+                s = 1;
+             elseif abs(A(r,r)) >= alpha*sigma
+                swap = 1;
+                m1 = k; m2 = r;
+                s = 1;
+             else
+                swap = 1;
+                m1 = k+1; m2 = r;
+                s = 2;
+             end
+          end
+
+          if swap
+             A( [m1, m2], : ) = A( [m2, m1], : );
+             L( [m1, m2], : ) = L( [m2, m1], : );
+             A( :, [m1, m2] ) = A( :, [m2, m1] );
+             L( :, [m1, m2] ) = L( :, [m2, m1] );
+             pp( [m1, m2] ) = pp( [m2, m1] );
+          end
+
+          if s == 1
+
+             D(k,k) = A(k,k);
+             A(k+1:n,k) = A(k+1:n,k)/A(k,k);
+             L(k+1:n,k) = A(k+1:n,k);
+             i = k+1:n;
+             A(i,i) = A(i,i) - A(i,k) * A(k,i);
+
+          elseif s == 2
+
+             E = A(k:k+1,k:k+1);
+             D(k:k+1,k:k+1) = E;
+             C = A(k+2:n,k:k+1);
+             temp = C/E;
+             L(k+2:n,k:k+1) = temp;
+             A(k+2:n,k+2:n) = A(k+2:n,k+2:n) - temp*C';
+
+         end
+
+         % Make diagonal real (see LINPACK User's Guide, p. 5.17).
+         for i=k+s:n
+             A(i,i) = real(A(i,i));
+         end
+
+         if k+s <= n
+            rho = max(rho, max(max(abs(A(k+s:n,k+s:n)))) );
+         end
+
+      else  % Nothing to do.
+
+         s = 1;
+         D(k,k) = A(k,k);
+
+      end
+
+      k = k + s;
+
+      if k == n
+         D(n,n) = A(n,n);
+         break
+      end
+
+end
+
+if nargout >= 3, P = eye(n); P = P(pp,:); end
+rho = rho/normA;