#include <math.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <conio.h>


void printmat(void);

void eig1(void);
void eig2(void);
void eig2b(void);
void eig3(void);

void switch_km(void);
void cramer1(void);
void Rayleigh(void);

void eigenvectors(void);


FILE *pFile[5];
char filename[5][FILENAME_MAX];


double m[3][3];
double k[3][3];


double ccc,ddd;

long ju;

long jnum = 50;

long ii,ik,i,j,jj,kk,n;


double aa,a1,a2,a3;

double ev1,ev2,ev3;

double omega1,omega2,omega3;

double a11,a12,a21,a22;

double scale1,scale2,scale3;

double ks[4][4],x2[4];

double a[4][4],b[4],bb[4],e[4][4];

double det,det2;


const double pi=atan2(0.,-1.);
const double tpi=2.*pi;



void main()
{
    printf("\n\n threedof.cpp  ver 1.0 \n August 25, 2003\n by Tom Irvine  Email: tomirvine@aol.com \n");

	printf("\n\n This program finds the eigenvalues and eigenvectors for a ");
	printf("\n three-degree-of-freedom system.");
	printf("\n\n The equation of motion is:  \n M (d^2x/dt^2) + K x = 0 \n\n");

	n=3;

	strcpy(filename[0],"threedof.out");
	pFile[0]=fopen(filename[0],"w");

	printf("\n Assume symmetric mass and stiffness matrices. ");

	printf("\n\n Enter m11 \n");
	scanf("%lf",&m[0][0]);

	printf("\n\n Enter m12 \n");
	scanf("%lf",&m[0][1]);
	
	printf("\n\n Enter m13 \n");	
	scanf("%lf",&m[0][2]);

	printf("\n\n Enter m22 \n");
	scanf("%lf",&m[1][1]);

	printf("\n\n Enter m23 \n");
	scanf("%lf",&m[1][2]);
	
	printf("\n\n Enter m33 \n");	
	scanf("%lf",&m[2][2]);




	printf("\n\n Enter k11 \n");
	scanf("%lf",&k[0][0]);

	printf("\n\n Enter k12 \n");
	scanf("%lf",&k[0][1]);
	
	printf("\n\n Enter k13 \n");	
	scanf("%lf",&k[0][2]);

	printf("\n\n Enter k22 \n");
	scanf("%lf",&k[1][1]);

	printf("\n\n Enter k23 \n");
	scanf("%lf",&k[1][2]);
	
	printf("\n\n Enter k33 \n");	
	scanf("%lf",&k[2][2]);



	for(i=0; i<3; i++)
	{
	
		for(j=i+1; j<3; j++)
		{
    
		       m[j][i]=m[i][j];
		       k[j][i]=k[i][j];
		}

	}
	

    printmat();

//********************************************

    eig1();
	Rayleigh();
	ev1=ccc;

    e[0][0]=b[0];
    e[0][1]=b[1];
    e[0][2]=b[2];
	
//********************************************

	eig3();
	Rayleigh();
	ev3=ccc;

    e[1][0]=b[0];
    e[1][1]=b[1];
    e[1][2]=b[2];

//********************************************

	eig2();
	Rayleigh();
	ev2=ccc;

    e[2][0]=b[0];
    e[2][1]=b[1];
    e[2][2]=b[2];

	eig2b();

//********************************************


	double ttt;

    for(i=0; i<3; i++)
	{
		ttt = e[1][i];
		e[1][i]=e[2][i];
		e[2][i]=ttt;
	}

	printf("\n eig1 = %14.4f",ev1);
	printf("\n eig2 = %14.4f",ev2);
	printf("\n eig3 = %14.4f",ev3);


	fprintf(pFile[0],"\n eig1 = %14.4f",ev1);
	fprintf(pFile[0],"\n eig2 = %14.4f",ev2);
	fprintf(pFile[0],"\n eig3 = %14.4f",ev3);


	omega1=sqrt(ev1);
	omega2=sqrt(ev2);
	omega3=sqrt(ev3);



	printf("\n\n omega1 = %14.4f rad/sec \n",omega1);
	printf(" omega2 = %14.4f rad/sec\n",omega2);
	printf(" omega3 = %14.4f rad/sec\n",omega3);



	fprintf(pFile[0],"\n\n omega1 = %14.4f rad/sec\n",omega1);
	fprintf(pFile[0]," omega2 = %14.4f rad/sec\n",omega2);
	fprintf(pFile[0]," omega3 = %14.4f rad/sec\n",omega3);



	printf("\n\n fn1 = %14.4f Hz \n",omega1/tpi);
	printf(" fn2 = %14.4f Hz\n",omega2/tpi);
	printf(" fn3 = %14.4f Hz\n",omega3/tpi);


	fprintf(pFile[0],"\n\n fn1 = %14.4f Hz \n",omega1/tpi);
	fprintf(pFile[0]," fn2 = %14.4f Hz\n",omega2/tpi);
	fprintf(pFile[0]," fn3 = %14.4f Hz\n",omega3/tpi);

//********************************************

    eigenvectors();

//********************************************

	printf("\n\n The output text file is:  threedof.out \n");

   printf("\n Press any key to exit.\n\n");
   getch();

}
void printmat(void)
{
    printf("\n\n Mass matrix \n");
    fprintf(pFile[0],"\n\n Mass matrix \n");
	for(i=0;i<n;i++)
	{
		for(j=0;j<n;j++)
		{
			printf("%12.4lf  ",m[i][j]);
			fprintf(pFile[0],"%12.4lf  ",m[i][j]);
		}
		printf("\n");
		fprintf(pFile[0],"\n");
	}

    printf("\n\n Stiffness matrix \n");
    fprintf(pFile[0],"\n\n Stiffness matrix \n");
	for(i=0;i<n;i++)
	{
		for(j=0;j<n;j++)
		{
			printf("%12.4lf  ",k[i][j]);
			fprintf(pFile[0],"%12.4lf  ",k[i][j]);
		}
		printf("\n");
		fprintf(pFile[0],"\n");
	}
}
void eig1(void)
{

    x2[0]=1.;
	x2[1]=1.;
	x2[2]=1.;


	for(ju = 0; ju<= jnum; ju++)
    {

//		printf(" ju= %ld \n",ju);

		for(i=0; i<3; i++)
		{
			b[i]= m[i][0]*x2[0] + m[i][1]*x2[1] +m[i][2]*x2[2];
        }

	    cramer1();	

		b[0]=a1;
		b[1]=a2;
		b[2]=a3;

       for(i=0;i<n;++i)
       {
		   bb[i]=0.;

	        for(j=0;j<n;++j)
	        {
	             bb[i]+=m[i][j]*b[j];
	        }
       }
	   ccc=0.;

	   for(i=0;i<n;++i)
       {
            ccc+=bb[i]*b[i];
	   }

       ccc=sqrt(ccc);

	   x2[0]=a1/ccc;
	   x2[1]=a2/ccc;
	   x2[2]=a3/ccc;

    }


//****************

    Rayleigh();

}
void eig2(void)
{

	b[0]=1.;

	double det = e[0][1]*e[1][2]-e[0][2]*e[1][1];


    b[1]=(   -e[0][0]*e[1][2]+e[0][2]*e[1][0]   )/det;

	b[2]=(   -e[0][1]*e[1][0]+e[0][0]*e[1][1]   )/det;


}
void eig3(void)
{

    x2[0]=1.;
	x2[1]=1.;
	x2[2]=1.;

    
    switch_km();

	for(ju = 0; ju<= jnum; ju++)
    {

//		printf(" ju= %ld \n",ju);

		for(i=0; i<3; i++)
		{
			b[i]= m[i][0]*x2[0] + m[i][1]*x2[1] +m[i][2]*x2[2];
        }

	    cramer1();	

		b[0]=a1;
		b[1]=a2;
		b[2]=a3;

       for(i=0;i<n;++i)
       {
		   bb[i]=0.;

	        for(j=0;j<n;++j)
	        {
	             bb[i]+=k[i][j]*b[j];
	        }
       }
	   ccc=0.;

	   for(i=0;i<n;++i)
       {
            ccc+=bb[i]*b[i];
	   }

       ccc=sqrt(ccc);

	   x2[0]=a1/ccc;
	   x2[1]=a2/ccc;
	   x2[2]=a3/ccc;

	}
	switch_km();
}

void Rayleigh(void)
{
	   for(i=0;i<n;++i)
       {
		   bb[i]=0.;

	        for(j=0;j<n;++j)
	        {
	             bb[i]+=m[i][j]*b[j];
	        }
       }
	   ccc=0.;

	   for(i=0;i<n;++i)
       {
            ccc+=bb[i]*b[i];
	   }

	   for(i=0;i<n;++i)
       {
		   bb[i]=0.;

	        for(j=0;j<n;++j)
	        {
	             bb[i]+=k[i][j]*b[j];
	        }
       }
	   ddd=0.;

	   for(i=0;i<n;++i)
       {
            ddd+=bb[i]*b[i];
	   }


      ccc=ddd/ccc;

}

void cramer1(void)
{
   double det,det1,det2,det3;

    det=0.;
   det1=0.;
   det2=0.;
   det3=0.;

//***

   det = k[0][0]*k[1][1]*k[2][2];
   det+= k[0][1]*k[1][2]*k[2][0];
   det+= k[0][2]*k[1][0]*k[2][1];

   det-= k[0][2]*k[1][1]*k[2][0];
   det-= k[0][1]*k[1][0]*k[2][2];
   det-= k[0][0]*k[1][2]*k[2][1];

//***

   det1 =    b[0]* k[1][1]* k[2][2];
   det1+= k[0][1]* k[1][2]*    b[2];
   det1+= k[0][2]*    b[1]* k[2][1];

   det1-= k[0][2]* k[1][1]*    b[2];
   det1-= k[0][1]*    b[1]* k[2][2];
   det1-=    b[0]* k[1][2]* k[2][1];

//***

   det2 = k[0][0]*    b[1]* k[2][2];
   det2+=    b[0]* k[1][2]* k[2][0];
   det2+= k[0][2]* k[1][0]*   b[2];

   det2-= k[0][2]*    b[1]* k[2][0];
   det2-=    b[0]* k[1][0]* k[2][2];
   det2-= k[0][0]* k[1][2]*    b[2];

//***

   det3 = k[0][0]* k[1][1]*    b[2];
   det3+= k[0][1]*    b[1]* k[2][0];
   det3+=    b[0]* k[1][0]* k[2][1];

   det3-=    b[0]* k[1][1]* k[2][0];
   det3-= k[0][1]* k[1][0]*    b[2];
   det3-= k[0][0]*    b[1]* k[2][1];

//***

   a1=det1/det;
   a2=det2/det;
   a3=det3/det;

//   printf("\n det = %12.4g \n",det);

}
void switch_km(void)
{	
	double temp[4][4];

	for(i=0; i<3; i++)
    {
		for(j=0; j<3; j++)
		{
          temp[i][j]=m[i][j];
			 m[i][j]=k[i][j];
		     k[i][j]=temp[i][j];
		}
    }

}
void eig2b(void)
{

	double aa,d,dd;

dd= +k[0][0]*k[1][1]*k[2][2]+k[0][1]*k[1][2]*k[2][0]+k[0][2]*k[1][0]*k[2][1]
	-k[0][0]*k[1][2]*k[2][1]-k[0][1]*k[1][0]*k[2][2]-k[0][2]*k[1][1]*k[2][0];


aa=  -m[0][0]*m[1][1]*m[2][2]-m[0][1]*m[1][2]*m[2][0]-m[0][2]*m[1][0]*m[2][1]
	 +m[0][0]*m[1][2]*m[2][1]+m[0][1]*m[1][0]*m[2][2]+m[0][2]*m[1][1]*m[2][0];

d=dd/aa;

    ev2=-d/(ev1*ev3);

//    printf(" aa=%12.4g  dd=%12.4g  d=%12.4g  ev1=%12.4g  ev2=%12.4g\n",aa,dd,d,ev1,ev2);

}
void eigenvectors(void)
{
	long n=3;


//********** 1 *************


       for(i=0;i<n;++i)
       {
			b[i]=e[0][i];

	   }


       for(i=0;i<n;++i)
       {	   
		   
		   bb[i]=0.;

	        for(j=0;j<n;++j)
	        {
	             bb[i]+=m[i][j]*b[j];

	        }
       }
	   ccc=0.;

	   for(i=0;i<n;++i)
       {
            ccc+=bb[i]*b[i];

	   }
	   for(i=0;i<n;++i)
       {
		   e[0][i] /=sqrt(ccc);

	   }
 

//********** 2 *************

       for(i=0;i<n;++i)
       {
			b[i]=e[1][i];
	   }

       for(i=0;i<n;++i)
       {

		   bb[i]=0.;

	        for(j=0;j<n;++j)
	        {
	             bb[i]+=m[i][j]*b[j];
	        }
       }
	   ccc=0.;

	   for(i=0;i<n;++i)
       {
            ccc+=bb[i]*b[i];
	   }
	   for(i=0;i<n;++i)
       {
		   e[1][i] /=sqrt(ccc);
	   }



//********** 3 *************


       for(i=0;i<n;++i)
       {
			b[i]=e[2][i];
	   }
		 
	   for(i=0;i<n;++i)
       {

		   bb[i]=0.;

	        for(j=0;j<n;++j)
	        {
	             bb[i]+=m[i][j]*b[j];
	        }
       }
	   ccc=0.;

	   for(i=0;i<n;++i)
       {
            ccc+=bb[i]*b[i];
	   }
	   for(i=0;i<n;++i)
       {
		   e[2][i] /=sqrt(ccc);
	   }

//*************************************



	     printf(         "\n\n Normalized Eigenvectors (column format)\n\n      1              2             3  \n\n");
        fprintf(pFile[0],"\n\n Normalized Eigenvectors (column format)\n\n      1              2             3  \n\n");
	
	for(i=0;i<n;i++)
	{
		for(j=0;j<n;j++)
		{
			printf("%12.4g   ",e[j][i]);
			fprintf(pFile[0],"%12.4g   ",e[j][i]);
		}
		printf("\n");
		fprintf(pFile[0],"\n");
	}

}