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

#define ZEROMAT(ccc) \
for(i=0; i<n; i++)\
{for(j=0;j<n; j++) {ccc[i][j]=0.;}}\

#define MATMUL(ccc,aaa,bbb) \
for(i=0; i<n; i++)\
{for(j=0; j<n; j++)\
{for(kk=0; kk<n; kk++)\
{ ccc[i][j]+=aaa[i][kk]*bbb[kk][j];	}}}


void printmat(void);
void data_input(void);
void eigenvalues(void);
void eigenvectors(void);
void participation(void);


double part1;
double part2;

double m[2][2];
double k[2][2];

double a,b,c;

double ev1,ev2;

double omega1,omega2;

double a11,a12,a21,a22;

double scale1,scale2;

double vector[2][2];
double vectorT[2][2];
double temp[2][2];
double mmm[2][2];
double QTMQ[2][2];

double mscale=1.;


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

long i,j,kk,n;

int iunit;

char mass_unit[20];
char stiffness_unit[20];


char s1[20],s2[20],s3[20],s4[30];

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

void main()
{

	strcpy(s1,"twodof.cpp");
	strcpy(s2,"ver 2.2");
	strcpy(s3,"March 6, 2006");
	strcpy(s4,"by Tom Irvine  Email: tomirvine@aol.com");


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

     printf("\n\n %s %s\n %s \n %s \n",s1,s2,s3,s4);
    fprintf(pFile[0],"\n\n %s %s\n %s \n %s \n",s1,s2,s3,s4);


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

	n=2;


	data_input();

    printmat();

    eigenvalues();

	eigenvectors();

	participation();


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

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

}
void printmat(void)
{
    printf("\n\n Mass matrix %s\n",mass_unit);
    fprintf(pFile[0],"\n\n Mass matrix %s\n",mass_unit);

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

    printf("\n\n Stiffness matrix %s\n",stiffness_unit);
    fprintf(pFile[0],"\n\n Stiffness matrix %s\n",stiffness_unit);

	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 participation(void)
{
	part1=vector[0][0]*(m[0][0]+m[0][1])   +  vector[1][0]*(m[1][0]+m[1][1]);
	part2=vector[0][1]*(m[0][0]+m[0][1])   +  vector[1][1]*(m[1][0]+m[1][1]);

    fprintf(pFile[0],"\n\n Particpation Factors \n\n");
	fprintf(pFile[0],"%12.4f   \n", part1);
	fprintf(pFile[0],"%12.4f   \n\n", part2);

    printf("\n\n Particpation Factors \n\n");
	printf("%12.4f   \n", part1);
	printf("%12.4f   \n\n", part2);



	part1=pow(part1,2.)/1.;
	part2=pow(part2,2.)/1.;

	if(iunit==1)
	{
		part1/=mscale;
		part2/=mscale;
	}


    fprintf(pFile[0],"\n\n Effective Modal Mass %s\n\n",mass_unit);
	fprintf(pFile[0],"%12.4f   \n", part1);
	fprintf(pFile[0],"%12.4f   \n\n", part2);

    printf("\n\n Effective Modal Mass  %s\n\n",mass_unit);
	printf("%12.4f   \n", part1);
	printf("%12.4f   \n\n", part2);

	double tm=part1+part2;

    printf("\n\n Total Modal Mass = %12.4f %s\n\n",tm,mass_unit);
    fprintf(pFile[0],"\n\n Total Modal Mass = %12.4f %s\n",tm,mass_unit);

}
void data_input(void)
{
	mscale=1.;

	strcpy(mass_unit,"(kg)");
	strcpy(stiffness_unit,"(N/m)");

	printf("\n Enter unit system \n");
	printf("\n 1=English  2=metric \n");
	scanf("%d",&iunit);

	if(iunit==1)
	{
		mscale=1./386.;

		strcpy(mass_unit,"(lbm)");
		strcpy(stiffness_unit,"(lbf/in)");
	}
	


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

	printf("\n mass unit is %s. \n",mass_unit);


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

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

	m[1][0]=m[0][1];

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


	printf("\n\n stiffness unit is %s. \n",stiffness_unit);


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

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

	k[1][0]=k[0][1];

}
void eigenvalues(void)
{
	
	a=m[0][0]*m[1][1]-m[0][1]*m[1][0];

    b=-m[0][0]*k[1][1]-m[1][1]*k[0][0]+m[0][1]*k[1][0]+m[1][0]*k[0][1];

	c=k[0][0]*k[1][1]-k[0][1]*k[1][0];

	ev1=(-b-sqrt(pow(b,2.)-4.*a*c) )/(2.*a);

	ev2=(-b+sqrt(pow(b,2.)-4.*a*c) )/(2.*a);



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

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


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


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

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



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

	fprintf(pFile[0],"\n\n fn1 = %14.4f Hz",omega1/tpi);
	fprintf(pFile[0],"\n fn2 = %14.4f Hz\n",omega2/tpi);
}
void eigenvectors(void)
{
	a11=k[0][0]-m[0][0]*ev1;
	a12=k[0][1]-m[0][1]*ev1;

    vector[0][0]=-a12/a11;
	vector[1][0]=1.;



	a11=k[0][0]-m[0][0]*ev2;
	a12=k[0][1]-m[0][1]*ev2;

    vector[0][1]=-a12/a11;
	vector[1][1]=1.;


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

	vectorT[0][0]=vector[0][0];
	vectorT[1][1]=vector[1][1];
	vectorT[0][1]=vector[1][0];
	vectorT[1][0]=vector[0][1];

    ZEROMAT(temp)
	MATMUL(temp,m,vector)
    MATMUL(QTMQ,vectorT,temp)

//    printf("\n\n Let Q = eigenvector matrix. \n");
    fprintf(pFile[0],"\n\n Let Q = eigenvector matrix. \n");

//    printf("\n\n QTMQ \n");
    fprintf(pFile[0],"\n\n QTMQ \n");
	for(i=0;i<n;i++)
	{
		for(j=0;j<n;j++)
		{
//			printf("%12.4f  ",QTMQ[i][j]);
			fprintf(pFile[0],"%12.4f  ",QTMQ[i][j]);
		}
//		printf("\n");
		fprintf(pFile[0],"\n");
	}

	scale1=1./sqrt(QTMQ[0][0]);
	scale2=1./sqrt(QTMQ[1][1]);

//	printf("\n\n scale1 = %12.4f ",scale1);
//	printf(  "\n scale2 = %12.4f \n",scale2);

	fprintf(pFile[0],"\n\n scale1 = %12.4f ",scale1);
	fprintf(pFile[0],"\n scale2 = %12.4f \n",scale2);



  	vector[0][0]*=scale1; 
	vector[1][0]*=scale1; 
	vector[0][1]*=scale2; 
	vector[1][1]*=scale2; 


	vectorT[0][0]=vector[0][0];
	vectorT[1][1]=vector[1][1];
	vectorT[0][1]=vector[1][0];
	vectorT[1][0]=vector[0][1];



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

    ZEROMAT(temp)
	ZEROMAT(QTMQ)
    MATMUL(temp,m,vector)
    MATMUL(QTMQ,vectorT,temp)

//    printf("\n Let P = normalized eigenvector matrix. \n");
    fprintf(pFile[0],"\n Let P = normalized eigenvector matrix. \n");
    
//	printf("\n\n Verify PTMP \n");
    fprintf(pFile[0],"\n\n Verify PTMP \n");
	for(i=0;i<n;i++)
	{
		for(j=0;j<n;j++)
		{
//			printf("%12.4f  ",QTMQ[i][j]);
			fprintf(pFile[0],"%12.4f  ",QTMQ[i][j]);
		}
//		printf("\n");
		fprintf(pFile[0],"\n");
	}


    ZEROMAT(temp)
	ZEROMAT(QTMQ)
    MATMUL(temp,k,vector)
    MATMUL(QTMQ,vectorT,temp)

//    printf("\n\n Verify PTKP \n");
    fprintf(pFile[0],"\n\n Verify PTKP \n");
	for(i=0;i<n;i++)
	{
		for(j=0;j<n;j++)
		{
//			printf("%12.4f  ",QTMQ[i][j]);
			fprintf(pFile[0],"%12.4f  ",QTMQ[i][j]);
		}
//		printf("\n");
		fprintf(pFile[0],"\n");
	}

}