import java.io.*;
class DK
{
public static double[][] calculate_glctm(double t, double d, double a, double al)
{
t = t * Math.PI/180;
al = al * Math.PI/180;
double glctm[][] = new double[4][4];
glctm[0][0] = Math.cos(t);
glctm[0][1] = -1 * Math.sin(t) * Math.cos(al);
glctm[0][2] = Math.sin(t) * Math.sin(al);
glctm[0][3] = a * Math.cos(t);
glctm[1][0] = Math.sin(t);
glctm[1][1] = Math.cos(t) * Math.cos(al);
glctm[1][2] = -1 * Math.sin(al) * Math.cos(t);
glctm[1][3] = a * Math.sin(t);
glctm[2][0] = 0;
glctm[2][1] = Math.sin(al);
glctm[2][2] = Math.cos(al);
glctm[2][3] = d;
glctm[3][0] = 0;
glctm[3][1] = 0;
glctm[3][2] = 0;
glctm[3][3] = 1;
return glctm;
}
public static double[][] matrix_mul(double m[][], double n[][])
{
double p[][] = new double[4][4];
int i,j,k;
for(i=0;i<4;i++)
{
for(j=0;j<4;j++)
{
for(k=0;k<4;k++)
{
p[i][j] += m[i][k] * n[k][j];
}
}
}
return p;
}
public static void main(String Args[])throws IOException
{
BufferedReader obj = new BufferedReader(new InputStreamReader(System.in));
int i,j,k;
//for theta k
double q[] = new double[4];
System.out.println("Enter value of q1:");
q[0] = Double.parseDouble(obj.readLine());
System.out.println("Enter value of q2:");
q[1] = Double.parseDouble(obj.readLine());
q[2] = 0;
System.out.println("Enter value of q4:");
q[3] = Double.parseDouble(obj.readLine());
//for dk
double d[] = new double[4];
System.out.println("Enter value of d1:");
d[0] = Double.parseDouble(obj.readLine());
d[1] = 0;
System.out.println("Enter value of d3:");
d[2] = Double.parseDouble(obj.readLine());
System.out.println("Enter value of d4:");
d[3] = Double.parseDouble(obj.readLine());
//for ak
double a[] = new double[4];
System.out.println("Enter value of a1:");
a[0] = Double.parseDouble(obj.readLine());
System.out.println("Enter value of a2:");
a[1] = Double.parseDouble(obj.readLine());
a[2] = 0;
a[3] = 0;
//for alpha
double al[] = new double[4];
al[0] = 180;
al[1] = 0;
al[2] = 0;
al[3] = 0;
//generate glctm
double t01[][] = new double[4][4];
double t12[][] = new double[4][4];
double t23[][] = new double[4][4];
double t34[][] = new double[4][4];
t01 = calculate_glctm(q[0],d[0],a[0],al[0]);
t12 = calculate_glctm(q[1],d[1],a[1],al[1]);
t23 = calculate_glctm(q[2],d[2],a[2],al[2]);
t34 = calculate_glctm(q[3],d[3],a[3],al[3]);
//calculate final
double t04[][] = new double[4][4];
t04 = matrix_mul(t01,t12);
t04 = matrix_mul(t04,t23);
t04 = matrix_mul(t04,t34);
//display op
System.out.println("Answer");
for(i=0;i<4;i++)
{
for(j=0;j<4;j++)
{
System.out.print(t04[i][j]+"\t");
}
System.out.println();
}
}
}
/* OUTPUT
c:\jdk1.3\bin\java.exe DK
Working Directory - C:\Documents and Settings\student\Desktop\
Class Path - .;c:\KawaEnt5.0\kawaclasses.zip;c:\jdk1.3\lib\tools.jar;c:\jdk1.3\jre\lib\rt.jar;c:\jdk1.3\jre\lib\i18n.jar
Enter value of q1:
45
Enter value of q2:
-60
Enter value of q4:
90
Enter value of d1:
877
Enter value of d3:
120
Enter value of d4:
200
Enter value of a1:
425
Enter value of a2:
375
Answer
0.9659258262890684 0.2588190451025207 8.659274570719354E-17 203.4632400908375
0.2588190451025207 -0.9659258262890684 -8.659274570719355E-17 662.7425668626834
6.123031769111886E-17 1.0605402120460133E-16 -1.0 557.0
0.0 0.0 0.0 1.0
Process Exit...
*/
Related
This content is really interesting. I have bookmarked it.
Do you allow guest posting on your website ?
I can write high quality posts for you. Let me know.