/*

Name: MECANISMO
Copyright: ESIME CULHUACAN
Author: ISRAEL CEBALLOS FLORES
Date: 12/11/11 13:34
Description: THIS PROGRAM COMPUTES THE FORCES ON THE JOINTS A AND B
*/
# include<cstdlib>
# include<iostream>
# include<cmath>
# include<conio.h>

# define PI 3.1415926

using namespace std;

const int num=5;
//FUNCION PRODUCTO VECTORIAL
float pvect(float A[num],float B[num],int s)
{
float C[3];
C[1]=A[2]*B[3]-A[3]*B[2];
C[2]=A[3]*B[1]-A[1]*B[3];
C[3]=A[1]*B[2]-A[2]*B[1];
return C[s];
}
//FUNCION IMPRIME VALOR DE MATRIZ
void imp_SELS(float A[num][num],float B[num],int n)
{
int i,j;
textcolor(GREEN);
printf("\n\n");
for(i=1;i<=n;i++){
textcolor(GREEN);
printf(" \xb3");
for(j=1;j<=n;j++)
printf("%10g",A[i][j]);
printf("\xb3");
textcolor(LIGHTRED);
printf("%10g \xb3\n",B[i]);
}
textcolor(BLACK);
}
//FUNCION SOLUCION DE SELS
float SELSgauss(float A[num][num],float B[num],int s)
{
float pivote,cte;
int i,j,k,cont,n=3;
// imp_SELS(A,B,3);
////////////////////////////////////////////////////////////////////////////
for(k=1;k<=n;k++){
pivote=A[k][k];
for(j=1;j<=n;j++)
A[k][j]=A[k][j]/pivote;
B[k]=B[k]/pivote;
for(i=1;i<=n;i++)
if(k!=i){
cte=A[i][k];
for(j=1;j<=n;j++)
A[i][j]=-cte*A[k][j]+A[i][j];
B[i]=-cte*B[k]+B[i];
}
 }
//imp_SELS(A,B,3);
////////////////////////////////////////////////////////////////////////////
return B[s];
}

int main()
{
textbackground(WHITE);
system("cls");
textcolor(LIGHTRED);
int i,j,k,cont,inc;
float M,w,a,b,c,rad,Vb,Ab,Va[2],Ag[3],W[3],H[3],av[3],bv[3],Rb[3],yt,rg[3],X[num][num],Y[num],l,m,n,o,p,q;
float Aa[3],aAgn[3],aAgt[3],temp[3],Ixn,Iyn,Izn,Ix,Iy,Iz,Ixy,Ixz,Iyz,ex[3],ey[3],ez[3],Mo[3],RA[3],RB[3],R,S;
for(i=1;i<=6;i++){
for(j=1;j<=6;j++)
X[i][j]=0;
}

cout<<"\n\t\t\t\tMECANISMO \n";textcolor(8);cout<<"\n Elaboro: Israel Ceballos Flores 15/11/2011\n"<<endl;

textcolor(BLUE);
cout<<" Ingrese la velocidad angular(rad/s): "; textcolor(LIGHTRED);cin>>w;
textcolor(BLUE);cout<<"\n Ingrese longitudes"<<endl;cout<<"\n a: "; textcolor(LIGHTRED);cin>>a;
textcolor(BLUE);cout<<"\n b: "; textcolor(LIGHTRED);cin>>b;
textcolor(BLUE);cout<<"\n c: "; textcolor(LIGHTRED);cin>>c;cout<<endl;
textcolor(BLUE);cout<<" Ingrese masa de la barra AB\n";cout<<"\n m: "; textcolor(LIGHTRED);cin>>M;
do{
gotoxy(3,20);
textcolor(BLUE);cout<<"Ingrese el incremento en (GRADOS): "; textcolor(LIGHTRED);cin>>inc;
if(inc%5==0){
cont=1;
system("cls");
}
else{
cont=0;
cout<<"ERROR..Ingrese otro incremento";
getch();
gotoxy(3,20);
cout<<" "<<"\n ";
}
}while(cont==0);
for(k=inc;k<=360;k=k+inc){
system("cls");
cout<<"\n\t\t\t\tMECANISMO \n";textcolor(8);cout<<"\n Elaboro: Israel Ceballos Flores 15/11/2011\n"<<endl;
textcolor(RED);
rad=k;
cout<<"\n\tAngulo de entrada= "<<rad<<endl;
rad=rad*(PI/180);
//VECTORES DE POSICION Y COORDENADAS
yt=sqrt(pow(b,2)-pow(a*sin(rad),2)-pow(a*cos(rad)+c,2));
av[1]=a*sin(rad);av[2]=0;av[3]=a*cos(rad);
Rb[1]=0;Rb[2]=yt;Rb[3]=-c;
for(i=1;i<=3;i++){
bv[i]=Rb[i]-av[i];
}
textcolor(12);
cout<<"\n Coordenadas:\n"<<endl;
textcolor(1);
cout<<"\tO(0,0,"<<c<<")"<<endl;
cout<<"\tA("<<a*sin(rad)<<",0,"<<c+a*cos(rad)<<")"<<endl;
cout<<"\tB(0,"<<yt<<",0)"<<endl;
 textcolor(LIGHTRED);
cout<<"\n Vectores de posicion:\n"<<endl;
textcolor(1);
cout<<"\n\ta= "<<av[1]<<"i "<<av[3]<<"k"<<endl;
cout<<"\n\tRb= "<<Rb[2]<<"j "<<Rb[3]<<"k "<<endl;
cout<<"\n\tb= "<<bv[1]<<"i "<<bv[2]<<"j "<<bv[3]<<"k "<<endl;

//VELOCIDAD ANGULAR Y VB Y VA
X[2][1]=bv[1]; X[2][2]=bv[2]; X[2][3]=bv[3]; Y[2]=0;
X[1][1]=yt; X[1][2]=a*sin(rad); X[1][3]=0; Y[1]=w*a*sin(rad);
X[3][1]=0; X[3][2]=a*cos(rad)+c; X[3][3]=yt; Y[3]=w*a*cos(rad);
for(i=1;i<=3;i++)
W[i]=SELSgauss(X,Y,i);
textcolor(LIGHTRED);
cout<<"\n Velocidad Angular"<<endl;
textcolor(1);
cout<<"\n\tW= "<<W[1]<<"i "<<W[2]<<"j "<<W[3]<<"k "<<endl;
cout<<"\n\t|W|= "<<sqrt(pow(W[1],2)+pow(W[2],2)+pow(W[3],2))<<endl;
Vb=(a*cos(rad)+c)*W[1]-a*sin(rad)*W[3];
cout<<"\n\tVelocidad de B = "<<Vb<<endl;
//ACELERACION ANGULAR
l=-W[2]*(a*cos(rad)+c)-yt*W[3];
m=(a*cos(rad)+c)*W[1]-a*sin(rad)*W[3];
n=yt*W[1]+a*sin(rad)*W[2];
o=-m*W[3]+n*W[2];
p=-n*W[1]+l*W[3];
q=-l*W[2]+m*W[1];
X[3][1]=bv[1]; X[3][2]=bv[2]; X[3][3]=bv[3]; Y[3]=0;
X[2][1]=0; X[2][2]=-(a*cos(rad)+c);X[2][3]=-yt; Y[2]=(pow(w,2)*a*sin(rad)-o);
X[1][1]=yt; X[1][2]=a*sin(rad); X[1][3]=0; Y[1]=(pow(w,2)*a*cos(rad)-q);

for(i=1;i<=3;i++)
H[i]=SELSgauss(X,Y,i);
textcolor(LIGHTRED);
cout<<"\n Aceleracion Angular"<<endl;
textcolor(1);
cout<<"\n\tH= "<<H[1]<<"i "<<H[2]<<"j "<<H[3]<<"k \n"<<endl;
Ab=p-a*sin(rad)*H[3]+(a*cos(rad)+c)*H[1];
cout<<"\tAceleracion de B = "<<Ab<<endl;
//ACELERACION DE CENTRO DE GRAVEDAD G
rg[1]=bv[1]/2;rg[2]=bv[2]/2;rg[3]=bv[3]/2;
for(i=1;i<=3;i++)
temp[i]=pvect(W,rg,i);
for(i=1;i<=3;i++)
aAgn[i]=pvect(W,temp,i);
for(i=1;i<=3;i++)
aAgt[i]=pvect(H,rg,i);
Ag[1]=-pow(w,2)*a*sin(rad)+aAgn[1]+aAgt[1];
Ag[2]=aAgn[2]+aAgt[2];
Ag[3]=-pow(w,2)*a*cos(rad)+aAgn[3]+aAgt[3];
textcolor(LIGHTRED);
cout<<"\n Aceleracion de G"<<endl;
textcolor(1);
cout<<"\n\tAg= "; textcolor(1);cout<<Ag[1]<<"i "<<Ag[2]<<"j "<<Ag[3]<<"k \n"<<endl;
//MOMENTOS DE INERCIA PRINCIPALES
Ixn=(M*pow(b,2)/12);
Izn=Ixn;
Iyn=0;
ey[1]=(-a*sin(rad)/b);ey[2]=yt/b;ey[3]=-(a*cos(rad)+c)/b;
ex[2]=-sqrt(pow(ey[1],2)/(pow(ey[1],2)+pow(ey[2],2)));
ex[1]=-(ey[2]/ey[1])*ex[2];
ex[3]=0;
 for(i=1;i<=3;i++)
ez[i]=pvect(ex,ey,i);
Ix=Ixn*pow(ex[1],2)+Izn*pow(ez[1],2);
Iy=Ixn*pow(ex[2],2)+Izn*pow(ez[2],2);
Iz=Ixn*pow(ex[3],2)+Izn*pow(ez[3],2);
Ixy=-(Ixn*ex[1]*ex[2]+Izn*ez[1]*ez[2]);
Ixz=-(Ixn*ex[1]*ex[3]+Izn*ez[1]*ez[3]);
Iyz=-(Ixn*ex[2]*ex[3]+Izn*ez[2]*ez[3]);

///ECUACION DE MOMENTOS
Mo[1]=Ix*H[1]+W[2]*W[3]*(Iz-Iy)+Ixy*(W[3]*W[1]-H[2])-Ixz*(H[3]+W[1]*W[2])-Iyz*(pow(W[2],2)-pow(W[3],2));

Mo[2]=Iy*H[2]+W[3]*W[1]*(Ix-Iz)+Iyz*(W[1]*W[2]-H[3])-Ixy*(H[1]+W[3]*W[2])-Ixy*(pow(W[3],2)-pow(W[1],2));

Mo[3]=Iz*H[3]+W[1]*W[2]*(Iy-Ix)+Ixz*(W[2]*W[3]-H[1])-Iyz*(H[2]+W[1]*W[3])-Ixy*(pow(W[1],2)-pow(W[2],2));

//REACCIONES
rg[1]=bv[1]/2;rg[2]=bv[2]/2;rg[3]=bv[3]/2;
cout<<"\n\tRg= "<<rg[1]<<"i "<<rg[2]<<"j "<<rg[3]<<"k \n"<<endl;
RA[2]=M*Ag[2];
RA[1]=(Mo[3]+rg[1]*RA[2])/(2*rg[2])+(M*Ag[1])/2;
RB[1]=M*Ag[1]-RA[1];
S=Mo[1]-rg[3]*RA[2];
R=M*Ag[3]+M*32.2;
RA[3]=-S/(2*rg[2])+R/2;
RB[3]=R-RA[3];
RB[2]=0;
textcolor(LIGHTRED);
cout<<" Reaciones en A y B"<<endl;
textcolor(1);cout<<"\n\tA= ";textcolor(1);cout<<RA[1]<<"i "<<RA[2]<<"j "<<RA[3]<<"k \n"<<endl;
cout<<"\n\tB= "<<RB[1]<<"i "<<RB[3]<<"k \n\n\n"<<endl;
textcolor(5);
system("pause");
system("cls");
}
}