From 0292b52cf7c6c8dfe157013ad488d6c5a26cb3eb Mon Sep 17 00:00:00 2001 From: Aditya Girish Date: Tue, 12 Nov 2024 11:23:32 -0600 Subject: [PATCH] Added graphics client for SIM rigid body. (#1779) * added SIM_singlerigidbody * Added matrix operations file. * Imported libraries and declared variables. * New objects and more defined values. * Declared more variables and added AzElRot. function. * Sockets and file connections 1 * Adding to variable server. * Variables for the variable server. * Variable parsing added. * Vertex model for crewModule; need to change to sphere later. * Body and vantage positioning functions. * Drawing functions to create the model. * Finished drawing functions for world. * Changed some of the object names and variable names for sync. * Added Makefile. * Added POM file. * Graphics client starter added to input file. * Changed paths in S_define to reflect current file structure. * More accurate cd error message in input file. * Color hardcoded for testing purposes and getting the sim running. * S_overrides changed to include graphics cleaning for each sim execution. * S_define formatted for better readability. * Changed shape to a dodecahedron. * Path changed for input file. * Airport commit * New edges for dodecahedron. * Triangle normals for the dodecahedron changed. * Sim variable parsing fixed for functionality. * Edges of dodecahedron fixed. * Changed paint color for the shape to change based on location. * Deleted unnecessary JViewport library. * Increased length of x, y, and z axis lines. * Increased vantage distance. * Fixed triangles and normals. * Commented on the edges. * Deleted the notes file. * Removed trick.frame_long_on() from splashdown SIM's realtime.py in modified data directory. * Deleted sims directory (including images sub directory). * Removed unnecessary body.hh and body.cpp from subdirectory. * Removed unnecessary files. --------- Co-authored-by: Wallace Co-authored-by: AdityaGirish --- trick_sims/SIM_singlerigidbody/S_overrides.mk | 12 + .../models/singlerigidbodygraphics/Makefile | 6 + .../models/singlerigidbodygraphics/pom.xml | 121 ++++ .../main/java/trick/matrixops/MatrixOps.java | 152 +++++ .../java/trick/srbdisplay/SRBDisplay.java | 628 ++++++++++++++++++ 5 files changed, 919 insertions(+) create mode 100644 trick_sims/SIM_singlerigidbody/models/singlerigidbodygraphics/Makefile create mode 100644 trick_sims/SIM_singlerigidbody/models/singlerigidbodygraphics/pom.xml create mode 100644 trick_sims/SIM_singlerigidbody/models/singlerigidbodygraphics/src/main/java/trick/matrixops/MatrixOps.java create mode 100644 trick_sims/SIM_singlerigidbody/models/singlerigidbodygraphics/src/main/java/trick/srbdisplay/SRBDisplay.java diff --git a/trick_sims/SIM_singlerigidbody/S_overrides.mk b/trick_sims/SIM_singlerigidbody/S_overrides.mk index e1f6cccd..abd95c9b 100644 --- a/trick_sims/SIM_singlerigidbody/S_overrides.mk +++ b/trick_sims/SIM_singlerigidbody/S_overrides.mk @@ -1,2 +1,14 @@ TRICK_CFLAGS += -Imodels TRICK_CXXFLAGS += -Imodels + +.PHONY: clean_graphics + +all: models/singlerigidbodygraphics/build/SingleRigidBodyDisplay.jar + +spotless: clean_graphics + +models/singlerigidbodygraphics/build/SingleRigidBodyDisplay.jar: + ${MAKE} -C ./models/singlerigidbodygraphics + +clean_graphics: + ${MAKE} -C ./models/singlerigidbodygraphics clean diff --git a/trick_sims/SIM_singlerigidbody/models/singlerigidbodygraphics/Makefile b/trick_sims/SIM_singlerigidbody/models/singlerigidbodygraphics/Makefile new file mode 100644 index 00000000..d3e0cbc0 --- /dev/null +++ b/trick_sims/SIM_singlerigidbody/models/singlerigidbodygraphics/Makefile @@ -0,0 +1,6 @@ + +all: + mvn package + +clean: + rm -rf build diff --git a/trick_sims/SIM_singlerigidbody/models/singlerigidbodygraphics/pom.xml b/trick_sims/SIM_singlerigidbody/models/singlerigidbodygraphics/pom.xml new file mode 100644 index 00000000..9aa0f442 --- /dev/null +++ b/trick_sims/SIM_singlerigidbody/models/singlerigidbodygraphics/pom.xml @@ -0,0 +1,121 @@ + + + + 4.0.0 + + trick-java + trick-java + 23.0.0-beta + + trick-java + + https://github.com/nasa/trick + + + UTF-8 + 1.8 + 1.8 + + + + + junit + junit + 4.13.1 + test + + + + + + SingleRigidBodyDisplay + + build + + + org.apache.maven.plugins + maven-javadoc-plugin + 3.1.1 + + ${java.home}/bin/javadoc + ../../share/doc/trick/java + + + + + + + + + + maven-clean-plugin + 3.1.0 + + + + + maven-resources-plugin + 3.0.2 + + + + maven-compiler-plugin + 3.8.0 + + + -g + -Xlint:unchecked + -Xlint:deprecation + + + + + + + org.apache.maven.plugins + maven-jar-plugin + 3.1.0 + + + + true + lib/ + trick.SRBDisplay + + + + + + + maven-surefire-plugin + 2.22.1 + + + + maven-install-plugin + 2.5.2 + + + + maven-deploy-plugin + 2.8.2 + + + + + maven-site-plugin + 3.7.1 + + + + + + + + \ No newline at end of file diff --git a/trick_sims/SIM_singlerigidbody/models/singlerigidbodygraphics/src/main/java/trick/matrixops/MatrixOps.java b/trick_sims/SIM_singlerigidbody/models/singlerigidbodygraphics/src/main/java/trick/matrixops/MatrixOps.java new file mode 100644 index 00000000..463b303b --- /dev/null +++ b/trick_sims/SIM_singlerigidbody/models/singlerigidbodygraphics/src/main/java/trick/matrixops/MatrixOps.java @@ -0,0 +1,152 @@ +// This file contains helper functions for the matrix operations necessary for the graphics to be generated. + +package trick.matrixOps; + + + +public class MatrixOps { // Defines the matrix operation class. + + public static void printMatrix(double M[][]) { // Prints out the matrix. + int M_rows = M.length; + int M_cols = M[0].length; + for (int i = 0; i < M_rows; i++) { + for (int j = 0; j < M_cols; j++) + System.out.print(" " + M[i][j]); + System.out.println(); + } + } + + public static void printDVector(double V[]) { // Prints out a vector passed as a double. + System.out.print("("); + for (int i = 0; i < V.length; i++) { + System.out.print(" " + V[i]); + } + System.out.println(")"); + } + + public static void printIVector(int V[]) { // Prints out a vector passed as an integer. + System.out.print("("); + for (int i = 0; i < V.length; i++) { + System.out.print(" " + V[i]); + } + System.out.println(")"); + } + + public static void MtimesM( double R[][], double A[][], double B[][]) { // Multiplies two matrices together. + int A_rows = A.length; + int A_cols = A[0].length; + int B_rows = B.length; + int B_cols = B[0].length; + int R_rows = R.length; + int R_cols = R[0].length; + + if (A_cols != B_rows) { // Checks if the matrices can be multiplied. + System.out.println( "\nNot possible to multiply matrixes,"); + System.out.println("where the first has " + A_cols + " columns,"); + System.out.println("and the second has " + B_rows + "rows."); + return; + } + if ((R_rows != A_rows) || (R_cols != B_cols)) { // Checks if the defined result matrix is the wrong size. + System.out.println( "\n Result matrix is wrong size."); + return; + } + + for (int i = 0; i < A_rows; i++) { // Multiplies the two matrices together. + for (int j = 0; j < B_cols; j++) { + R[i][j] = 0.0; + for (int k = 0; k < B_rows; k++) + R[i][j] += A[i][k] * B[k][j]; + } + } + } + + public static void MtimesV( double R[], double M[][], double V[]) { // Multiplies a matrix with a vector. + int M_rows = M.length; + int M_cols = M[0].length; + int V_rows = V.length; + + if (M_cols != V_rows) { // Checks if the matrix and the vector can be multiplied together. + System.out.println( "\nNot possible to multiply matrix and vector,"); + System.out.println( "where the matrix has " + M_cols + " columns,"); + System.out.println("and the vector has " + V_rows + " elements."); + return; + } + + if (R.length != M.length) { // Checks if the defined result vector is the wrong size. + System.out.println( "\n Result vector is wrong size."); + return; + } + + for (int i =0; i < M_rows ; i++) { // Multiplies the vector with the matrix. + R[i] = 0.0; + for (int j =0; j < M_cols ; j++) { + R[i] += M[i][j] * V[j]; + } + } + return; + } + + public static void VplusV(double R[], double A[], double B[]) { // Adds two matrices together. + if ((A.length != B.length) || (A.length != R.length)) { + System.out.println( "\n MatrixOps::VplusV : Vectors are not the same size."); + } + for (int i=0; i Math.PI) vantageAzimuth -= Math.PI; + if (vantageAzimuth < -Math.PI) vantageAzimuth += Math.PI; + vantageElevation -= (dy * Math.PI) / getHeight(); + if (vantageElevation > Math.toRadians( 89.0)) vantageElevation = Math.toRadians( 89.0); + if (vantageElevation < Math.toRadians(-89.0)) vantageElevation = Math.toRadians(-89.0); + setAzElRotation(worldToVantageRotation, vantageAzimuth, vantageElevation); + repaint(); + } + + public void setAzElRotation(double RotationMatrix[][], double azimuth, double elevation) { // Sets the azimuth and elevation rotation. + double Rotation_about_Y[][] = { + { Math.cos(elevation), 0.0, Math.sin(elevation)}, + { 0.0, 1.0, 0.0}, + {-Math.sin(elevation), 0.0, Math.cos(elevation)} + }; + + double Rotation_about_Z[][] = { + {Math.cos(azimuth), -Math.sin(azimuth), 0.0}, + {Math.sin(azimuth), Math.cos(azimuth), 0.0}, + { 0.0, 0.0, 1.0} + }; + MatrixOps.MtimesM( RotationMatrix, Rotation_about_Y, Rotation_about_Z); + } + + public void worldToScreenPoint( int result[], double V_world[]) { + double V_vantage[] = new double[3]; + // Tranform vector in world coordinates to vantage coordinates. + MatrixOps.MtimesV(V_vantage, worldToVantageRotation, V_world); + // Perspective projection of point in 3D vantage coordinates to 2D screen coordinates. + double perspective_scale = screen_half_width/(Math.tan(beta)*(vantageDistance-V_vantage[0])); + result[0] = (int)(perspective_scale * V_vantage[1] + screen_half_width); + result[1] = (int)(screen_half_height - perspective_scale * V_vantage[2]); + } + + public void setBodyPos( double x, double y, double z) { + bodyPos[0] = x; + bodyPos[1] = y; + bodyPos[2] = z; + } + + public void drawLineSegmentInWorld(Graphics2D g, Color color, double start[], double end[]) { + g.setPaint(color); + int start_screen[] = {0, 0}; + int end_screen[] = {0, 0}; + worldToScreenPoint( start_screen, start); + worldToScreenPoint( end_screen, end); + g.drawLine( start_screen[0], start_screen[1], end_screen[0], end_screen[1]); + } + + public void drawLabelInWorld(Graphics2D g, Color color, double loc_world[], String s) { + g.setPaint(color); + int loc_screen[] = {0, 0}; + worldToScreenPoint( loc_screen, loc_world); + g.drawString ( s, loc_screen[0], loc_screen[1]); + } + + private void doDrawing( Graphics g) { + Graphics2D g2d = (Graphics2D) g; + + int width = getWidth(); + int height = getHeight(); + + screen_half_width = (width/2); + screen_half_height = (height/2); + + g2d.setPaint(Color.WHITE); + g2d.fillRect(0, 0, width, height); + + + // Draw the vehicle. + + // Transform the vehicle vertices from body -> world, apply the vehicle position offset, and then to 2D screen points. + for (int i=0; i World -> Vantage + MatrixOps.MtimesV(veh_unit_normals_world[i], bodyToWorldRotation, veh_unit_normals_body[i]); + MatrixOps.MtimesV(veh_unit_normals_vantage[i], worldToVantageRotation, veh_unit_normals_world[i]); + + // Render the triangle only if it's facing us. + double facing_angle = MatrixOps.VdotV(veh_unit_normals_vantage[i], LOS_vantage); + if ( (facing_angle > 0.0) && (facing_angle < Math.toRadians(90))) { + + // Calculate the diffuse reflection intensity. + double neg_illumination_vector[] = {0.0, 0.0, 0.0}; + MatrixOps.Vscale(neg_illumination_vector, illumination_vector, -1.0); + double diffuse_intensity = MatrixOps.VdotV(neg_illumination_vector, veh_unit_normals_world[i]); + if (diffuse_intensity < 0.0) diffuse_intensity = 0.0; + + // Proportion of the total light due to ambient light. + // (1.0 - ambient) is the proportion of reflected light. + double ambient = 0.8; // Must be between 0.0 and 1.0. + + // The color intensity is a combination of ambient light intensity, + // and diffuse reflection intensity. + double color_intensity = (ambient + (1.0 - ambient) * diffuse_intensity); + + g2d.setPaint( new Color( (int)(bodyFillColor.getRed() * color_intensity), + (int)(bodyFillColor.getGreen() * color_intensity), + (int)(bodyFillColor.getBlue() * color_intensity))); + + // Draw the triangle. + int triangle_poly_x[] = {0, 0, 0}; + int triangle_poly_y[] = {0, 0, 0}; + // For each point of the triangle. + for (int j=0; j < 3; j++) { + + triangle_poly_x[j] = veh_vrtx_screen[ veh_triangles[i][j] ][0]; + triangle_poly_y[j] = veh_vrtx_screen[ veh_triangles[i][j] ][1]; + + } + g2d.fillPolygon(triangle_poly_x, triangle_poly_y, 3); + } + } + + // Draw Wireframe Model + + g2d.setPaint( bodyLineColor ); + + for (int i = 0; i\n" + + "----------------------------------------------------------------------\n" + ); + } + + public static void main(String[] args) throws IOException { + + String host = "localHost"; + int port = 0; + String bodyImageFile = null; + + int ii = 0; + while (ii < args.length) { + switch (args[ii]) { + case "-help" : + case "--help" : { + printHelpText(); + System.exit(0); + } break; + default : { + port = (Integer.parseInt(args[ii])); + } break; + } + ++ii; + } + + if (port == 0) { + System.out.println("No variable server port specified."); + printHelpText(); + System.exit(0); + } + + SingleRigidBodyView singleRigidBodyView = new SingleRigidBodyView(); + + SRBDisplay sd = new SRBDisplay(singleRigidBodyView); + sd.setVisible(true); + + double f_init_x = 0.0; + double f_init_y = 0.0; + double f_init_z = 0.0; + + double accX = 0.0; + double accY = 0.0; + double accZ = 0.0; + + double angleX = 0.0; + double angleY = 0.0; + double angleZ = 0.0; + + double f_angleX = 0.0; + double f_angleY = 0.0; + double f_angleZ = 0.0; + + double forceX = 0.0; + double forceY = 0.0; + double forceZ = 0.0; + + double omegaX = 0.0; + double omegaY = 0.0; + double omegaZ = 0.0; + + double omegaDotX = 0.0; + double omegaDotY = 0.0; + double omegaDotZ = 0.0; + + double posX = 0.0; + double posY = 0.0; + double posZ = 0.0; + + double Rxx = 0.0; + double Rxy = 0.0; + double Rxz = 0.0; + double Ryx = 0.0; + double Ryy = 0.0; + double Ryz = 0.0; + double Rzx = 0.0; + double Rzy = 0.0; + double Rzz = 0.0; + + double velX = 0.0; + double velY = 0.0; + double velZ = 0.0; + + System.out.println("Connecting to: " + host + ":" + port); + sd.connectToServer(host, port); + + sd.out.writeBytes("trick.var_set_client_tag(\"SRBDisplay\") \n" + + "trick.var_pause() \n" + + + "trick.var_add(\"dyn.body.FORCE_INIT[0]\") \n" + // 36 + "trick.var_add(\"dyn.body.FORCE_INIT[1]\") \n" + + "trick.var_add(\"dyn.body.FORCE_INIT[2]\") \n" + + + "trick.var_add(\"dyn.body.acc[0]\") \n" + + "trick.var_add(\"dyn.body.acc[1]\") \n" + + "trick.var_add(\"dyn.body.acc[2]\") \n" + + + "trick.var_add(\"dyn.body.angle[0]\") \n" + + "trick.var_add(\"dyn.body.angle[1]\") \n" + + "trick.var_add(\"dyn.body.angle[2]\") \n" + + + "trick.var_add(\"dyn.body.angle_force[0]\") \n" + + "trick.var_add(\"dyn.body.angle_force[1]\") \n" + + "trick.var_add(\"dyn.body.angle_force[2]\") \n" + + + "trick.var_add(\"dyn.body.force[0]\") \n" + + "trick.var_add(\"dyn.body.force[1]\") \n" + + "trick.var_add(\"dyn.body.force[2]\") \n" + + + "trick.var_add(\"dyn.body.omega[0]\") \n" + + "trick.var_add(\"dyn.body.omega[1]\") \n" + + "trick.var_add(\"dyn.body.omega[2]\") \n" + + + "trick.var_add(\"dyn.body.omegaDot[0]\") \n" + + "trick.var_add(\"dyn.body.omegaDot[1]\") \n" + + "trick.var_add(\"dyn.body.omegaDot[2]\") \n" + + + "trick.var_add(\"dyn.body.pos[0]\") \n" + + "trick.var_add(\"dyn.body.pos[1]\") \n" + + "trick.var_add(\"dyn.body.pos[2]\") \n" + + + "trick.var_add(\"dyn.body.rotate[0][0]\") \n" + + "trick.var_add(\"dyn.body.rotate[0][1]\") \n" + + "trick.var_add(\"dyn.body.rotate[0][2]\") \n" + + "trick.var_add(\"dyn.body.rotate[1][0]\") \n" + + "trick.var_add(\"dyn.body.rotate[1][1]\") \n" + + "trick.var_add(\"dyn.body.rotate[1][2]\") \n" + + "trick.var_add(\"dyn.body.rotate[2][0]\") \n" + + "trick.var_add(\"dyn.body.rotate[2][1]\") \n" + + "trick.var_add(\"dyn.body.rotate[2][2]\") \n" + + + "trick.var_add(\"dyn.body.vel[0]\") \n" + + "trick.var_add(\"dyn.body.vel[1]\") \n" + + "trick.var_add(\"dyn.body.vel[2]\") \n" + + + "trick.var_ascii() \n" + + "trick.var_cycle(0.1) \n" + + "trick.var_unpause()\n" ); + + sd.out.flush(); + + sd.drawSRBView(); + + Boolean go = true; + + while (go) { + + String field[]; + + try { + String line; + line = sd.in.readLine(); + field = line.split("\t"); + + f_init_x = Double.parseDouble( field[1] ); + f_init_y = Double.parseDouble( field[2] ); + f_init_z = Double.parseDouble( field[3] ); + + accX = Double.parseDouble( field[4] ); + accY = Double.parseDouble( field[5] ); + accZ = Double.parseDouble( field[6] ); + + angleX = Double.parseDouble( field[7] ); + angleY = Double.parseDouble( field[8] ); + angleZ = Double.parseDouble( field[9] ); + + f_angleX = Double.parseDouble( field[10] ); + f_angleY = Double.parseDouble( field[11] ); + f_angleZ = Double.parseDouble( field[12] ); + + forceX = Double.parseDouble( field[13] ); + forceY = Double.parseDouble( field[14] ); + forceZ = Double.parseDouble( field[15] ); + + omegaX = Double.parseDouble( field[16] ); + omegaY = Double.parseDouble( field[17] ); + omegaZ = Double.parseDouble( field[18] ); + + omegaDotX = Double.parseDouble( field[19] ); + omegaDotY = Double.parseDouble( field[20] ); + omegaDotZ = Double.parseDouble( field[21] ); + + posX = Double.parseDouble( field[22] ); + posY = Double.parseDouble( field[23] ); + posZ = Double.parseDouble( field[24] ); + + Rxx = Double.parseDouble( field[25] ); + Rxy = Double.parseDouble( field[26] ); + Rxz = Double.parseDouble( field[27] ); + Ryx = Double.parseDouble( field[28] ); + Ryy = Double.parseDouble( field[29] ); + Ryz = Double.parseDouble( field[30] ); + Rzx = Double.parseDouble( field[31] ); + Rzy = Double.parseDouble( field[32] ); + Rzz = Double.parseDouble( field[33] ); + + velX = Double.parseDouble( field[34] ); + velY = Double.parseDouble( field[35] ); + velZ = Double.parseDouble( field[36] ); + + // Set the body position + singleRigidBodyView.setBodyPos(posX, posY, posZ); + + singleRigidBodyView.setBodyToWorldRotation( Rxx, Rxy, Rxz, + Ryx, Ryy, Ryz, + Rzx, Rzy, Rzz ); + + } catch (IOException | NullPointerException e ) { + go = false; + } + sd.drawSRBView(); + } + } +} \ No newline at end of file