Implementing HCTM in Java

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

*/

Leave a Reply

This site uses Akismet to reduce spam. Learn how your comment data is processed.