/* diags.cp */

/*
	written by:
	Robert M. Delaney
	Emeritus Professor of Physics
	Saint Louis University
	September, 2003
*/
extern  double zeroDouble;
static double dabs(double x)
{
    if(x<0)
        x = -x;
    
    return x;
    
}/* dabs */

/*
	diagonalizes a symmetric matrix using the Jacobi method. Only the upper triangular
	part, including the diagonal, needs to be inputted.

	M is the matrix to be diagonalized. the matrix EigenM is returned with its columns the
	eigenvectors. n is the size of the matrices, which should have memory dynamically allocated by
	the calling program. eps is a measure of the acceptable largest off-diagonal element
	in absolute value.

	Basic method:
	For a symmetric sub-matrix of dimension 2
	A = x  z
		z  y
		
	where x an y are M[i][i] and M[j][j] (i<j)
	z is M[i][j] and M[j][i]
	where z is largest off-diagonal element in absolute value
	
	U A U' , where here ' means transpose, diagonalizes A where:
	
	lambda = |z|
	mu = (x-y)/2
	f = sign(mu)*lambda/sqrt(lambda^2 + mu^2)  (if mu=0 then sign(mu)=1)
	s = f/sqrt(2*(1+sqrt(1-f^2)))
	c = sqrt(1-s^2)
	
	Then with pf=1 if largest off-diagonal element is positive and pf=-1 otherwise
	U =  c      pf*s
		-pf*s   c
		
	U' = c     -pf*s
		 pf*s   c
		 
	A is diagonalized with new diagonal elements:
	x*c^2 + y*s^2 + 2*lambda*s*c
	x*s^2 + y*c^2 - 2*lambda*s*c
	
	Apply an expanded U (with suitably inserted 1's and 0's) by M = U M U' to generate a new M
	
	We start with EigenM matrix a unit matrix, and multiply it from the right by expanded U'
	At the end the columns of the EigenM matrix are the eigenvectors
	
	repeat until absolute value of largest off-diagonal element is less that some given eps
	
*/


#include <math.h>
#include "diags.h"


static inline double absD(double z)
{
	if(z<0)
		z = -z;
	return z;
	
}/* absD */


void diags(double **M, double **EigenM, long n, double eps)
{
	long		i, j, is, js, pf;
	double		f, lambda, mu, sig, norm, maxValue, maxValue1, temp;
	double		s, c, x, y, s2, c2;
	double		M1, M2;
	
	if(n==1 || eps<=0)
		return;
		
	// make EigenM a unit matrix
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
		{
			if(i==j)
				EigenM[i][j] = 1;
			else
				EigenM[i][j] = 0;
		}
		
	// fill in lower triangular part of M
	for(i=0;i<n-1;i++)
		for(j=i+1;j<n;j++)
			M[j][i] = M[i][j];
		
	// find norm value as the largest off-diagonal element in absolute value
	norm = 0;
	for(i=0;i<n-1;i++)
		for(j=i+1;j<n;j++)
			if(absD(M[i][j])>norm)
				norm = absD(M[i][j]);
		
	if(norm==0) // M is already diagonal
		return;
	
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
			M[i][j] = M[i][j]/norm;
			
	do
	{
		maxValue = absD(M[0][1]);
		is = 0;
		js = 1;
		
		// find the largest off-diagonal element in absolute value
		for(i=0;i<n-1;i++)
			for(j=i+1;j<n;j++)
			{
				maxValue1 = absD(M[i][j]);
				if(maxValue1>maxValue)
				{
					is = i;
					js = j;
					maxValue = maxValue1;
				}
			}
			
		if(maxValue>=eps)
		{
			lambda = absD(M[is][js]);
			
			mu = (M[is][is] - M[js][js])/2;
			
			if(mu>=0)
				sig = 1;
			else
				sig = -1;
				
			pf = 1;
			if(M[is][js]<0)
				pf = -1;
				
			f = sig*lambda/sqrt(lambda*lambda + mu*mu);
			
			s = f/sqrt(2*(1+sqrt(1-f*f)));
			c = sqrt(1-s*s);
			
			x = M[is][is];
			y = M[js][js];
			
			temp = 2*lambda*s*c;
			s2 = s*s;
			c2 = c*c;
			M[is][is] = x*c2 + y*s2 + temp;
			M[js][js] = x*s2 + y*c2 - temp;
			M[is][js] = M[js][is] = 0;
			
			for(i=0;i<n;i++)
			{
				if(i!=is && i!=js)
				{
					M1 = M[i][is];
					M2 = M[i][js];
					M[i][is] = c*M1 + pf*s*M2;
					M[i][js] = -pf*s*M1 + c*M2;
					
					M[is][i] = M[i][is];
					M[js][i] = M[i][js];
				}
				
				M1 = EigenM[i][is];
				M2 = EigenM[i][js];
				EigenM[i][is] = c*M1 + pf*s*M2;
				EigenM[i][js] = -pf*s*M1 + c*M2;
			}
		}
	}
	while (maxValue >= eps);
		
	for(i=0;i<n;i++)
		M[i][i] = norm*M[i][i];
    
    for(i=0;i<n;++i)
        for(j=0;j<n;++j)
        {
            if(dabs(M[i][j])<zeroDouble)
                M[i][j] = 0;
            
            if(dabs(EigenM[i][j])<zeroDouble)
                EigenM[i][j] = 0;
        }
	
}/* diags */
