import java.io.*;
class hctm
{
public static void main(String args[])throws IOException
{
BufferedReader obj = new BufferedReader(new InputStreamReader(System.in));
System.out.println("Which operation you want to perform");
System.out.println("1. Rotation - Yaw");
System.out.println("2. Rotation - Pitch");
System.out.println("3. Rotation - Roll");
System.out.println("4. Translation");
int ch = Integer.parseInt(obj.readLine());
rot r = new rot();
switch(ch)
{
case 1: r.yaw(); break;
case 2: r.pitch();break;
case 3: r.roll();break;
case 4: trans t = new trans();break;
default: System.out.println("Invalid option");
}
}
}
class rot
{
public void yaw()throws IOException
{
BufferedReader obj = new BufferedReader(new InputStreamReader(System.in));
System.out.println("\n Enter angle");
double a= Double.parseDouble(obj.readLine());
a= a*3.14/180;
double r1[][]={{1,0,0,0},{0,Math.cos(a),-1* Math.sin(a),0},{0,Math.sin(a),Math.cos(a),0},{0,0,0,1}};
System.out.println("Enter mobile coordinate frame");
int m[] = new int[4];
m[0]= Integer.parseInt(obj.readLine());
m[1]= Integer.parseInt(obj.readLine());
m[2]= Integer.parseInt(obj.readLine());
m[3]= Integer.parseInt(obj.readLine());
double res[] = new double[4];
for(int i=0;i<4;i++)
{
for(int j=0;j<4;j++)
{
res[i]= res[i]+r1[i][j]*m[j];
}
}
System.out.println("Answer:"+res[0]+" "+res[1]+" "+res[2]+" "+res[3]);
}
public void pitch()throws IOException
{
BufferedReader obj = new BufferedReader(new InputStreamReader(System.in));
System.out.println("\n Enter angle");
double a= Double.parseDouble(obj.readLine());
a= a*3.14/180;
double r1[][]={{Math.cos(a),0,Math.sin(a),0},{0,1,0,0},{-1*Math.sin(a),0,Math.cos(a),0},{0,0,0,1}};
System.out.println("Enter mobile coordinate frame");
int m[] = new int[4];
m[0]= Integer.parseInt(obj.readLine());
m[1]= Integer.parseInt(obj.readLine());
m[2]= Integer.parseInt(obj.readLine());
m[3]= Integer.parseInt(obj.readLine());
double res[] = new double[4];
for(int i=0;i<4;i++)
{
for(int j=0;j<4;j++)
{
res[i]= res[i]+r1[i][j]*m[j];
}
}
System.out.println("Answer:"+res[0]+" "+res[1]+" "+res[2]+" "+res[3]);
}
public void roll()throws IOException
{
BufferedReader obj = new BufferedReader(new InputStreamReader(System.in));
System.out.println("\n Enter angle");
double a= Double.parseDouble(obj.readLine());
a= a*3.14/180;
double r1[][]={{Math.cos(a),-1*Math.sin(a),0,0},{Math.sin(a),Math.cos(a),0,0},{0,0,1,0},{0,0,0,1}};
System.out.println("Enter mobile coordinate frame");
double m[] = new double[4];
m[0]= Double.parseDouble(obj.readLine());
m[1]= Double.parseDouble(obj.readLine());
m[2]= Double.parseDouble(obj.readLine());
m[3]= Double.parseDouble(obj.readLine());
double res[] = new double[4];
for(int i=0;i<4;i++)
{
for(int j=0;j<4;j++)
{
res[i]= res[i]+r1[i][j]*m[j];
}
}
System.out.println("Answer:"+res[0]+" "+res[1]+" "+res[2]+" "+res[3]);
}
}
class trans
{
public trans()throws IOException
{
BufferedReader obj = new BufferedReader(new InputStreamReader(System.in));
int t[] = new int[4];
System.out.println("Enter Tnasformation matrix");
t[0] = Integer.parseInt(obj.readLine());
t[1] = Integer.parseInt(obj.readLine());
t[2] = Integer.parseInt(obj.readLine());
t[3] = Integer.parseInt(obj.readLine());
double r1[][]={{1,0,0,t[0]},{0,1,0,t[1]},{0,0,1,t[2]},{0,0,0,t[3]}};
System.out.println("Enter mobile coordinate frame");
int m[] = new int[4];
m[0]= Integer.parseInt(obj.readLine());
m[1]= Integer.parseInt(obj.readLine());
m[2]= Integer.parseInt(obj.readLine());
m[3]= Integer.parseInt(obj.readLine());
double res[] = new double[4];
for(int i=0;i<4;i++)
{
for(int j=0;j<4;j++)
{
res[i]= res[i]+r1[i][j]*m[j];
}
}
System.out.println("Answer:"+res[0]+" "+res[1]+" "+res[2]+" "+res[3]);
}
}
/* OUTPUT
Which operation you want to perform
1. Rotation - Yaw
2. Rotation - Pitch
3. Rotation - Roll
4. Translation
1
Enter angle
60
Enter mobile coordinate frame
2
0
3
1
Answer:2.0 -2.597279518477033 1.5013790670246174 1.0
Which operation you want to perform
1. Rotation - Yaw
2. Rotation - Pitch
3. Rotation - Roll
4. Translation
2
Enter angle
60
Enter mobile coordinate frame
2
0
3
1
Answer:3.598198896493445 0.0 -0.2301406119600713 1.0
Which operation you want to perform
1. Rotation - Yaw
2. Rotation - Pitch
3. Rotation - Roll
4. Translation
3
Enter angle
45
Enter mobile coordinate frame
0.5
1.5
0.6
1
Answer:-0.706543637074449 1.4144949943034828 0.6 1.0
Which operation you want to perform
1. Rotation - Yaw
2. Rotation - Pitch
3. Rotation - Roll
4. Translation
4
Enter Tnasformation matrix
3
0
5
1
Enter mobile coordinate frame
0
0
1
1
Answer:3.0 0.0 6.0 1.0
*/
Related