text: #ifndef LINEQN_H
  #define LINEQN_H
  /*
  Szymon Rusinkiewicz
  Princeton University
  
  lineqn.h
  Solution of systems of linear equations and eigenvalue decomposition.
  Some are patterned after the code in Numerical Recipes, but with a bit of
  the fancy C++ template thing happening.
  */
  
  #include <cmath>
  #include <algorithm>
  using std::sqrt;
  using std::fabs;
  
  #ifndef likely
  #  if !defined(__GNUC__) || (__GNUC__ == 2 && __GNUC_MINOR__ < 96)
  #    define likely(x) (x)
  #    define unlikely(x) (x)
  #  else
  #    define likely(x)   (__builtin_expect((x), 1))
  #    define unlikely(x) (__builtin_expect((x), 0))
  #  endif
  #endif
  
  
  // LU decomposition
  template <class T, int N>
  static inline bool ludcmp(T a[N][N], int indx[N], T *d = NULL)
  {
  	int i, j, k;
  	T vv[N];
  
  	if (d)
  		*d = 1;
  	for (i = 0; i < N; i++) {
  		T big = 0.0;
  		for (j = 0; j < N; j++) {
  			T tmp = fabs(a[i][j]);
  			if (tmp > big)
  				big = tmp;
  		}
  		if (big == 0.0)
  			return false;
  		vv[i] = 1.0 / big;
  	}
  	for (j = 0; j < N; j++) {
  		for (i = 0; i < j; i++) {
  			T sum = a[i][j];
  			for (k = 0; k < i; k++)
  				sum -= a[i][k]*a[k][j];
  			a[i][j]=sum;
  		}
  		T big = 0.0;
  		int imax = j;
  		for (i = j; i < N; i++) {
  			T sum = a[i][j];
  			for (k = 0; k < j; k++)
  				sum -= a[i][k]*a[k][j];
  			a[i][j] = sum;
  			T tmp = vv[i] * fabs(sum);
  			if (tmp > big) {
  				big = tmp;
  				imax = i;
  			}
  		}
  		if (imax != j) {
  			for (k = 0; k < N; k++)
  				std::swap(a[imax][k], a[j][k]);
  			if (d)
  				*d = -(*d);
  			vv[imax] = vv[j];
  		}
  		indx[j] = imax;
  		if (a[j][j] == 0.0)
  			return false;
  		if (j != N-1) {
  			T tmp = 1.0/(a[j][j]);
  			for (i = j+1; i < N; i++)
  				a[i][j] *= tmp;
  		}
  	}
  	return true;
  }
  
  
  // Backsubstitution after ludcmp
  template <class T, int N>
  static inline void lubksb(T a[N][N], int indx[N], T b[N])
  {
  	int ii = -1, i, j;
  	for (i = 0; i < N; i++) {
  		int ip = indx[i];
  		T sum = b[ip];
  		b[ip] = b[i];
  		if (ii != -1)
  			for (j = ii; j < i; j++)
  				sum -= a[i][j] * b[j];
  		else if (sum)
  			ii = i;
  		b[i] = sum;
  	}
  	for (i = N-1; i >= 0; i--) {
  		T sum = b[i];
  		for (j = i+1; j < N; j++)
  			sum -= a[i][j] * b[j];
  		b[i] = sum / a[i][i];
  	}
  }
  
  
  // Perform LDL^T decomposition of a symmetric positive definite matrix.
  // Like Cholesky, but no square roots.  Overwrites lower triangle of matrix.
  template <class T, int N>
  static inline bool ldltdc(T A[N][N], T rdiag[N])
  {
  	T v[N-1];
  	for (int i = 0; i < N; i++) {
  		for (int k = 0; k < i; k++)
  			v[k] = A[i][k] * rdiag[k];
  		for (int j = i; j < N; j++) {
  			T sum = A[i][j];
  			for (int k = 0; k < i; k++)
  				sum -= v[k] * A[j][k];
  			if (i == j) {
  				if (unlikely(sum <= T(0)))
  					return false;
  				rdiag[i] = T(1) / sum;
  			} else {
  				A[j][i] = sum;
  			}
  		}
  	}
  
  	return true;
  }
  
  
  // Solve Ax=B after ldltdc
  template <class T, int N>
  static inline void ldltsl(T A[N][N], T rdiag[N], T B[N], T x[N])
  {
  	int i;
  	for (i = 0; i < N; i++) {
  		T sum = B[i];
  		for (int k = 0; k < i; k++)
  			sum -= A[i][k] * x[k];
  		x[i] = sum * rdiag[i];
  	}
  	for (i = N - 1; i >= 0; i--) {
  		T sum = 0;
  		for (int k = i + 1; k < N; k++)
  			sum += A[k][i] * x[k];
  		x[i] -= sum * rdiag[i];
  	}
  }
  
  
  // Eigenvector decomposition for real, symmetric matrices,
  // a la Bowdler et al. / EISPACK / JAMA
  // Entries of d are eigenvalues, sorted smallest to largest.
  // A changed in-place to have its columns hold the corresponding eigenvectors.
  // Note that A must be completely filled in on input.
  template <class T, int N>
  static inline void eigdc(T A[N][N], T d[N])
  {
  	// Householder
  	T e[N];
  	for (int j = 0; j < N; j++) {
  		d[j] = A[N-1][j];
  		e[j] = 0.0;
  	}
  	for (int i = N-1; i > 0; i--) {
  		T scale = 0.0;
  		for (int k = 0; k < i; k++)
  			scale += std::fabs(d[k]);
  		if (scale == 0.0) {
  			e[i] = d[i-1];
  			for (int j = 0; j < i; j++) {
  				d[j] = A[i-1][j];
  				A[i][j] = A[j][i] = 0.0;
  			}
  			d[i] = 0.0;
  		} else {
  			T h(0);
  			T invscale = T(1.0 / scale);
  			for (int k = 0; k < i; k++) {
  				d[k] *= invscale;
  				h += d[k]*d[k];
  			}
  			T f = d[i-1];
  			T g = (f > 0.0) ? -std::sqrt(h) : std::sqrt(h);
  			e[i] = scale * g;
  			h -= f * g;
  			d[i-1] = f - g;
  			for (int j = 0; j < i; j++)
  				e[j] = 0.0;
  			for (int j = 0; j < i; j++) {
  				f = d[j];
  				A[j][i] = f;
  				g = e[j] + f * A[j][j];
  				for (int k = j+1; k < i; k++) {
  					g += A[k][j] * d[k];
  					e[k] += A[k][j] * f;
  				}
  				e[j] = g;
  			}
  			f = 0.0;
  			T invh = T(1.0 / h);
  			for (int j = 0; j < i; j++) {
  				e[j] *= invh;
  				f += e[j] * d[j];
  			}
  			T hh = f / (h + h);
  			for (int j = 0; j < i; j++)
  				e[j] -= hh * d[j];
  			for (int j = 0; j < i; j++) {
  				f = d[j];
  				g = e[j];
  				for (int k = j; k < i; k++)
  					A[k][j] -= f * e[k] + g * d[k];
  				d[j] = A[i-1][j];
  				A[i][j] = 0.0;
  			}
  			d[i] = h;
  		}
  	}
  
  	for (int i = 0; i < N-1; i++) {
  		A[N-1][i] = A[i][i];
  		A[i][i] = 1.0;
  		T h = d[i+1];
  		if (h != 0.0) {
  			T invh = T(1.0 / h);
  			for (int k = 0; k <= i; k++)
  				d[k] = A[k][i+1] * invh;
  			for (int j = 0; j <= i; j++) {
  				T g = 0.0;
  				for (int k = 0; k <= i; k++)
  					g += A[k][i+1] * A[k][j];
  				for (int k = 0; k <= i; k++)
  					A[k][j] -= g * d[k];
  			}
  		}
  		for (int k = 0; k <= i; k++)
  			A[k][i+1] = 0.0;
  	}
  	for (int j = 0; j < N; j++) {
  		d[j] = A[N-1][j];
  		A[N-1][j] = 0.0;
  	}
  	A[N-1][N-1] = 1.0;
  
  	// QL
  	for (int i = 1; i < N; i++)
  		e[i-1] = e[i];
  	e[N-1] = 0.0;
  	T f = 0.0, tmp = 0.0;
  	const T eps = T(pow(2.0, -52.0));
  	for (int l = 0; l < N; l++) {
  		tmp = std::max< T >(tmp, std::fabs(d[l]) + std::fabs(e[l]));
  		int m = l;
  		while (m < N) {
  			if (std::fabs(e[m]) <= eps * tmp)
  				break;
  			m++;
  		}
  		if (m > l) {
  			do {
  				T g = d[l];
  				T p = (d[l+1] - g) / (e[l] + e[l]);
  				T r = T(hypot(p, 1.0));
  				if (p < 0.0)
  					r = -r;
  				d[l] = e[l] / (p + r);
  				d[l+1] = e[l] * (p + r);
  				T dl1 = d[l+1];
  				T h = g - d[l];
  				for (int i = l+2; i < N; i++)
  					d[i] -= h; 
  				f += h;
  				p = d[m];   
  				T c = 1.0, c2 = 1.0, c3 = 1.0;
  				T el1 = e[l+1], s = 0.0, s2 = 0.0;
  				for (int i = m - 1; i >= l; i--) {
  					c3 = c2;
  					c2 = c;
  					s2 = s;
  					g = c * e[i];
  					h = c * p;
  					r = T(hypot(p, e[i]));
  					e[i+1] = s * r;
  					s = e[i] / r;
  					c = p / r;
  					p = c * d[i] - s * g;
  					d[i+1] = h + s * (c * g + s * d[i]);
  					for (int k = 0; k < N; k++) {
  						h = A[k][i+1];
  						A[k][i+1] = s * A[k][i] + c * h;
  						A[k][i] = c * A[k][i] - s * h;
  					}
  				}
  				p = -s * s2 * c3 * el1 * e[l] / dl1;
  				e[l] = s * p;
  				d[l] = c * p;
  			} while (std::fabs(e[l]) > eps * tmp);
  		}
  		d[l] += f;
  		e[l] = 0.0;
  	}
  
  	// Sort
  	for (int i = 0; i < N-1; i++) {
  		int k = i;
  		T p = d[i];
  		for (int j = i+1; j < N; j++) {
  			if (d[j] < p) {
  				k = j;
  				p = d[j];
  			}
  		}
  		if (k == i)
  			continue;
  		d[k] = d[i];
  		d[i] = p;
  		for (int j = 0; j < N; j++) {
  			p = A[j][i];
  			A[j][i] = A[j][k];
  			A[j][k] = p;
  		}
  	}
  }
  
  
  // x <- A * d * A' * b
  template <class T, int N>
  static inline void eigmult(T A[N][N],
  			   T d[N],
  			   T b[N],
  			   T x[N])
  {
  	T e[N];
  	for (int i = 0; i < N; i++) {
  		e[i] = 0.0;
  		for (int j = 0; j < N; j++)
  			e[i] += A[j][i] * b[j];
  		e[i] *= d[i];
  	}
  	for (int i = 0; i < N; i++) {
  		x[i] = 0.0;
  		for (int j = 0; j < N; j++)
  			x[i] += A[i][j] * e[j];
  	}
  }
  
  #endif
  