#include <stdio.

h>
#include <stdlib.h>
#include <math.h>
#define pi 3.1411592653
void damecoordenadas(float a[3]);
float encontrarR(float a[3]);
float encontarS(float r, float a[3]);
float encuentra1ang(float a[3]);
float encuentraang3(float s, float r);
float encuentraang2(float s, float r, float t3);
float encuentraang4(float R[3][3]);
void matRH(float A1[3][3],float A2[3][3],float A3[3][3],float t1,float t2,float
t3);
void mat1mat2(float R[3][3],float A1[3][3], float A2[3][3]);
void R30(float R[3][3],float A3,float R2[3][3]);
void Trans30(float R[3][3], float RT[3][3]);
int main()
{
float a[3], R[3][3],R2[3][3],t1,r,s,t3,t2,A1[3][3],A2[3][3],A3[3][3];
printf("PROGRAMA SOLUCION A EXAMEN SEGUNDO PARCIAL ROBOTICA...\n\n\n");
damecoordenadas(a);
r=encontrarR(a);
printf("\n r= %f\n",r);
s=encontarS(r,a);
printf(" s= %f\n",s);
t1=((encuentra1ang(a))*360)/(2*pi);
printf(" Theta\"1\"= %f \n",t1);
t3=(encuentraang3(s,r)*360)/(2*pi);
printf(" Theta\"3\"= %f \n",t3);
t2=encuentraang2( s,r,t3);
printf(" Theta\"2\"= %f \n",t2);
matRH(A1,A2,A3,t1,t2,t3);
mat1mat2(R,A1,A2);
//R30(R,A3,R2);

return 0;
}
void damecoordenadas(float a[3])
{
int i=0;
printf(" Introduce coordenada X : \n ",i);
scanf("%f",&a[0]);
i=i++;
printf(" Introduce coordenada Y : \n ",i);
scanf("%f",&a[1]);
i=i++;
printf(" Introduce coordenada Z : \n ",i);
scanf("%f",&a[2]);

return;
}
float encuentra1ang(float a[3])
{
float t;
t=atan2(a[1],a[0]);
return t;
}
float encontrarR(float a[3])
{
float r;
r=sqrt(pow(a[0],2)+pow(a[1],2));
return r;
}
float encontarS(float r, float a[3])
{
float s;
s=(a[2]-2);
return s;
}
float encuentraang3(float s, float r)
{
float L,t3,d;
L=sqrt(pow(r,2)+pow(s,2));
d=(pow(r,2)+pow(s,2)-8)/8;
printf(" Cos(Theta\"3\")= %f\n",d);
t3=atan2(-1*sqrt(1-pow(d,2)),-1*d);
return t3;
}
float encuentraang2(float s, float r, float t3)
{
float g1,B1,B2,t2,g2;
t3=((t3)*(pi))/180;
g1=atan2(-s,r);
g2=((g1)*(180))/3.141592653589;
printf("\n Gamma= %f",g2);
B1=atan2(2*sin(t3),(2+2*cos(t3)));
B2=(B1)*(180)/3.141592653589;
printf("\n Beta= %f\n",B2);
t2=g2-B2;
return t2;
}
void matRH(float A1[3][3],float A2[3][3],float A3[3][3],float t1,float t2,float
t3)
{
float al1,al2,al3;
printf("\n De los parametros de D-H Introduce alpha1: \n");
scanf("%f", &al1);

printf("\n De los parametros de D-H Introduce alpha2: \n");

scanf("%f", &al2);
printf("\n De los parametros de D-H Introduce alpha3: \n");
scanf("%f", &al3);
A1[0][0]=cos(t1);
A1[0][1]=(-1)*(sin(t1))*(cos(al1));
A1[0][2]=(sin(t1))*(sin(al1));
A1[1][0]=(sin(t1));
A1[1][1]=(cos(t1))*(cos(al1));
A1[1][2]=(-1)*(cos(t1))*(sin(al1));
A1[2][0]=0;
A1[2][1]=sin(al1);
A1[2][2]=cos(al1);
A2[0][0]=cos(t2);
A2[0][1]=(-1)*(sin(t2))*(cos(al2));
A2[0][2]=(sin(t2))*(sin(al2));
A2[1][0]=(sin(t2));
A2[1][1]=(cos(t2))*(cos(al2));
A2[1][2]=(-1)*(cos(t2))*(sin(al2));
A2[2][0]=0;
A2[2][1]=sin(al2);
A2[2][2]=cos(al2);
A3[0][0]=cos(t3);
A3[0][1]=(-1)*(sin(t3))*(cos(al3));
A3[0][2]=(sin(t3))*(sin(al3));
A3[1][0]=(sin(t3));
A3[1][1]=(cos(t3))*(cos(al3));
A3[1][2]=(-1)*(cos(t3))*(sin(al3));
A3[2][0]=0;
A3[2][1]=sin(al3);
A3[2][2]=cos(al3);
return;
}
void mat1mat2(float R[3][3],float A1[3][3], float A2[3][3])
{
}
void R30(float R[3][3],float A3,float R2[3][3])
{

//-.5
//1.5
//1
//4448480925