eigen.cpp

Computes the eigenvalues and eigenvectors of a real upper-Hessenberg Matrix using the QR method

#include <math.h>
#include "stdafx.h"
#include "drishti.h"
#include "Eigen.h"
#include "MemUtil.h"

Eigen::Eigen(){
}

void Eigen::cdivA(double ar, double ai, double br, double bi, double **A, int in1, int in2, int in3) {
	// Division routine for dividing one complex number into another:
	// This routine does (ar + ai)/(br + bi) and returns the results in the specified
	// elements of the A matrix.
	
	double s, ars, ais, brs, bis;
	
	s = fabs(br) + fabs(bi);
	ars = ar/s;
	ais = ai/s;
	brs = br/s;
	bis = bi/s;
	s = brs*brs + bis*bis;
	A[in1][in2] = (ars*brs + ais*bis)/s;
	A[in1][in3] = (-(ars*bis) + ais*brs)/s;
	return;
} // End cdivA

void Eigen::hqr2(double **A, int N, double **B, int low, int igh, double *wr, double *wi, int &ierr) {
/* Computes the eigenvalues and eigenvectors of a real upper-Hessenberg Matrix using 
	the QR method. */
	
	double norm = 0.0, r, t = 0.0, tst1, tst2, p, q, ra, s, sa, vi, vr, w, x, y, zz;
	int k = 0, l, m, mp2, en = igh, incrFlag = 1, its, itn = 30*N, enm2, na, notlas;
	int i, j, ii, jj;
	
	for (i = 0; i < N; i++){ // Store eigenvalues already isolated and compute matrix norm.
		for (j = k; j < N; j++)
			norm += fabs(A[i][j]);
		k = i;
		if ((i < low) || (i > igh)){
			wi[i] = 0.0;
			wr[i] = A[i][i];
		} //End if (i < low or i > igh)
	}  // End for i
	
	// Search next eigenvalues
	while (en >= low){
		
		if (incrFlag) { //Skip this part if incrFlag is set to 0 at very end of while loop
			its = 0;
			na = en - 1;
			enm2 = na - 1;
		} //End if (incrFlag)
		else
			incrFlag = 1;
		
		/*Look for single small sub-diagonal element for l = en step -1 until low */
		
		for (i = low; i <= en; i++){
			l = en + low - i;
			if (l == low)
				break;
			s = fabs(A[l - 1][l - 1]) + fabs(A[l][l]);
			if (s == 0.0)
				s = norm;
			tst1 = s;
			tst2 = tst1 + fabs(A[l][l - 1]);
			if (tst2 == tst1)
				break;
		} //End for i
		
		x = A[en][en];
		
		if (l == en){  //One root found
			wr[en] = A[en][en] = x + t;
			wi[en] = 0.0;
			en--;
			continue;
		} //End if (l == en)
		
		y = A[na][na];
		w = A[en][na]*A[na][en];
		
		if (l == na){  //Two roots found
			p = (-x + y)/2;
			q = p*p + w;
			zz = sqrt(fabs(q));
			x = A[en][en] = x + t;
			A[na][na] = y + t;
			if (q >= 0.0){//Real Pair
				zz = ((p < 0.0) ? (-1*zz + p) : (p + zz));
				wr[en] = wr[na] = x + zz;
				if (zz != 0.0)
					wr[en] = -(w/zz) + x;
				wi[en] = wi[na] = 0.0;
				x = A[en][na];
				s = fabs(x) + fabs(zz);
				p = x/s;
				q = zz/s;
				r = sqrt(p*p + q*q);
				p /= r;
				q /= r;
				for (j = na; j < N; j++){ //Row modification
					zz = A[na][j];
					A[na][j] = q*zz + p*A[en][j];
					A[en][j] = -(p*zz) + q*A[en][j];
				}//End for j
				for (j = 0; j <= en; j++){ // Column modification
					zz = A[j][na];
					A[j][na] = q*zz + p*A[j][en];
					A[j][en] = -(p*zz) + q*A[j][en];
				}//End for j
				for (j = low; j <= igh; j++){//Accumulate transformations
					zz = B[j][na];
					B[j][na] = q*zz + p*B[j][en];
					B[j][en] = -(p*zz) + q*B[j][en];
				}//End for j
			} //End if (q >= 0.0)
			else {//else q < 0.0
				wr[en] = wr[na] = x + p;
				wi[na] = zz;
				wi[en] = -zz;
			} //End else
			en--;
			en--;
			continue;
		}//End if (l == na)
		
		if (itn == 0){ //Set error; all eigenvalues have not converged after 30 iterations.
			ierr = en + 1;
			return;
		}//End if (itn == 0)
		
		if ((its == 10) || (its == 20)){ //Form exceptional shift
			t += x;
			for (i = low; i <= en; i++)
				A[i][i] += -x;
			s = fabs(A[en][na]) + fabs(A[na][enm2]);
			y = x = 0.75*s;
			w = -0.4375*s*s;
		} //End if (its equals 10 or 20)
		
		its++;
		itn--;
		
		/*Look for two consecutive small sub-diagonal elements. Do m = en - 2 to l in 
		increments of -1 */
		
		for (m = enm2; m >= l; m--){
			zz = A[m][m];
			r = -zz + x;
			s = -zz + y;
			p = (-w + r*s)/A[m + 1][m] + A[m][m + 1];
			q = -(zz + r + s) + A[m + 1][m + 1] ;
			r = A[m + 2][m + 1];
			s = fabs(p) + fabs(q) + fabs(r);
			p /= s;
			q /= s;
			r /= s;
			if (m == l)
				break;
			tst1 = fabs(p) * (fabs(A[m - 1][m - 1]) + fabs(zz) + fabs(A[m + 1][m + 1]));
			tst2 = tst1 + fabs(A[m][m - 1]) * (fabs(q) + fabs(r));
			if (tst1 == tst2)
				break;
		}//End for m
		
		mp2 = m + 2;
		
		for (i = mp2; i <= en; i++){
			A[i][i - 2] = 0.0;
			if (i == mp2)
				continue;
			A[i][i - 3] = 0.0;
		}//End for i
		
		/* Double qr step involving rows l to en and columns m to en. */
		
		for (i = m; i <= na; i++){
			notlas = ((i != na) ? 1 : 0);
			if (i != m){
				p = A[i][i - 1];
				q = A[i + 1][i - 1];
				r = ((notlas) ? A[i + 2][i - 1] : 0.0);
				x = fabs(p) + fabs(q) + fabs(r);
				if (x == 0.0)      //Drop through rest of for i loop
					continue;
				p /= x;
				q /= x;
				r /= x;
			} //End if (i != m)
			
			s = sqrt(p*p + q*q + r*r);
			if (p < 0.0)
				s = -s;
			
			if (i != m)
				A[i][i - 1] = -(s*x);
			else {
				if (l != m)
					A[i][i - 1] = -A[i][i - 1];
			}
			
			p += s;
			x = p/s;
			y = q/s;
			zz = r/s;
			q /= p;
			r /= p;
			k = ((i + 3 < en) ? i + 3 : en);
			
			if (notlas){ //Do row modification
				for (j = i; j < N; j++) {
					p = A[i][j] + q*A[i + 1][j] + r*A[i + 2][j];
					A[i][j] += -(p*x);
					A[i + 1][j] += -(p*y);
					A[i + 2][j] += -(p*zz);
				}//End for j
				
				for (j = 0; j <= k; j++) {//Do column modification
					p = x*A[j][i] + y*A[j][i + 1] + zz*A[j][i + 2];
					A[j][i] += -p;
					A[j][i + 1] += -(p*q);
					A[j][i + 2] += -(p*r);
				}//End for j
				
				for (j = low; j <= igh; j++) {//Accumulate transformations
					p = x*B[j][i] + y*B[j][i + 1] + zz*B[j][i + 2];
					B[j][i] += -p;
					B[j][i + 1] += -(p*q);
					B[j][i + 2] += -(p*r);
				} // End for j
			}//End if notlas
			
			else {
				for (j = i; j < N; j++) {//Row modification
					p = A[i][j] + q*A[i + 1][j];
					A[i][j] += -(p*x);
					A[i + 1][j] += -(p*y);
				}//End for j
				
				for (j = 0; j <= k; j++){//Column modification
					p = x*A[j][i] + y*A[j][i +1];
					A[j][i] += -p;
					A[j][i + 1] += -(p*q);
				}//End for j
				
				for (j = low; j <= igh; j++){//Accumulate transformations
					p = x*B[j][i] + y*B[j][i +1];
					B[j][i] += -p;
					B[j][i + 1] += -(p*q);
				}//End for j
				
			} //End else if notlas
		}//End for i
		incrFlag = 0;
		}//End while (en >= low)
		
		if (norm == 0.0)
			return;
		
		//Step from (N - 1) to 0 in steps of -1
		
		for (en = (N - 1); en >= 0; en--){
			p = wr[en];
			q = wi[en];
			na = en - 1;
			
			if (q > 0.0)
				continue;
			
			if (q == 0.0){//Real vector
				m = en;
				A[en][en] = 1.0;
				
				for (j = na; j >= 0; j--){
					w = -p + A[j][j];
					r = 0.0;
					for (ii = m; ii <= en; ii++)
						r += A[j][ii]*A[ii][en];
					
					if (wi[j] < 0.0){
						zz = w;
						s = r;
					}//End wi[j] < 0.0
					
					else {//wi[j] >= 0.0
						m = j;
						if (wi[j] == 0.0){
							t = w;
							if (t == 0.0){
								t = tst1 = norm;
								do {
									t *= 0.01;
									tst2 = norm + t;
								} while (tst2 > tst1);
							} //End if t == 0.0
							A[j][en] = -(r/t);
						}//End if wi[j] == 0.0
						
						else { //else wi[j] > 0.0; Solve real equations
							x = A[j][j + 1];
							y = A[j + 1][j];
							q = (-p + wr[j])*(-p + wr[j]) + wi[j]*wi[j];
							A[j][en] = t = (-(zz*r) + x*s)/q;
							A[j + 1][en] = ((fabs(x) > fabs(zz)) ? -(r + w*t)/x : -(s + y*t)/zz);
						}//End  else wi[j] > 0.0
						
						// Overflow control
						t = fabs(A[j][en]);
						if (t == 0.0)
							continue; //go up to top of for j loop
						tst1 = t;
						tst2 = tst1 + 1.0/tst1;
						if (tst2 > tst1)
							continue; //go up to top of for j loop
						for (ii = j; ii <= en; ii++)
							A[ii][en] /= t;
						
					}//End else wi[j] >= 0.0
					
				}//End for j
				
			}      //End q == 0.0
			
			else {//else q < 0.0, complex vector
				//Last vector component chosen imaginary so that eigenvector matrix is triangular
				m = na;
				
				if (fabs(A[en][na]) <= fabs(A[na][en]))
					cdivA(0.0, -A[na][en], -p + A[na][na], q, A, na, na, en);
				else {
					A[na][na] = q/A[en][na];
					A[na][en] = -(-p + A[en][en])/A[en][na];
				} //End else (abs(A[en][na] > abs(A[na][en])
				
				A[en][na] = 0.0;
				A[en][en] = 1.0;
				
				for (j = (na - 1); j >= 0; j--) {
					w = -p + A[j][j];
					sa = ra = 0.0;
					
					for (ii = m; ii <= en; ii++) {
						ra += A[j][ii]*A[ii][na];
						sa += A[j][ii]*A[ii][en];
					} //End for ii
					
					if (wi[j] < 0.0){
						zz = w;
						r = ra;
						s = sa;
						continue;
					} //End if (wi[j] < 0.0)
					
					//else wi[j] >= 0.0
					m = j;
					if (wi[j] == 0.0)
						cdivA(-ra, -sa, w, q, A, j, na, en);
					else {// else wi[j] > 0.0; solve complex equations
						x = A[j][j + 1];
						y = A[j + 1][j];
						vr = -(q*q) + (-p + wr[j])*(-p + wr[j]) + wi[j]*wi[j];
						vi = (-p + wr[j])*2.0*q;
						if ((vr == 0.0) && (vi == 0.0)){
							tst1 = norm*(fabs(w) + fabs(q) + fabs(x) + fabs(y) + fabs(zz));
							vr = tst1;
							do {
								vr *= 0.01;
								tst2 = tst1 + vr;
							} while (tst2 > tst1);
						} //End if vr and vi == 0.0
						cdivA(-(zz*ra) + x*r + q*sa, -(zz*sa + q*ra) + x*s, vr, vi, A, j, na, en);
						
						if (fabs(x) > (fabs(zz) + fabs(q))){
							A[j + 1][na] = (-(ra + w*A[j][na]) + q*A[j][en])/x;
							A[j + 1][en] = -(sa + w*A[j][en] + q*A[j][na])/x;
						}//End if
						else
							cdivA(-(r + y*A[j][na]), -(s + y*A[j][en]), zz, q, A, j + 1, na, en);
						
					}//End else wi[j] > 0.0
					
					t = ((fabs(A[j][na]) >= fabs(A[j][en])) ? fabs(A[j][na]) : fabs(A[j][en]));
					
					if (t == 0.0)
						continue; // go to top of for j loop