ExternalVendorCode/trick
1
0
Fork 0
mirror of https://github.com/nasa/trick.git synced 2024-12-18 12:56:26 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Added graphics client for SIM rigid body. (#1779)

Browse Source 
* 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 <bnwalla1@scooby.trick.gov>
Co-authored-by: AdityaGirish <aditya.girish@nasa.gov>
This commit is contained in:
Aditya Girish 2024-11-12 11:23:32 -06:00 committed by GitHub
parent aecf6a06fd
commit 0292b52cf7
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
5 changed files with 919 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
trick_sims/SIM_singlerigidbody/S_overrides.mk
View File

@ -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

6
trick_sims/SIM_singlerigidbody/models/singlerigidbodygraphics/Makefile Normal file
View File

@ -0,0 +1,6 @@
all:
mvn package
clean:
rm -rf build

121
trick_sims/SIM_singlerigidbody/models/singlerigidbodygraphics/pom.xml Normal file
View File

@ -0,0 +1,121 @@
<?xml version="1.0" encoding="UTF-8"?>
<project xmlns="http://maven.apache.org/POM/4.0.0" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:schemaLocation="http://maven.apache.org/POM/4.0.0 http://maven.apache.org/xsd/maven-4.0.0.xsd">
<modelVersion>4.0.0</modelVersion>
<groupId>trick-java</groupId>
<artifactId>trick-java</artifactId>
<version>23.0.0-beta</version>
<name>trick-java</name>
<url>https://github.com/nasa/trick</url>
<properties>
<project.build.sourceEncoding>UTF-8</project.build.sourceEncoding>
<maven.compiler.source>1.8</maven.compiler.source>
<maven.compiler.target>1.8</maven.compiler.target>
</properties>
<dependencies>
<dependency>
<groupId>junit</groupId>
<artifactId>junit</artifactId>
<version>4.13.1</version>
<scope>test</scope>
</dependency>
</dependencies>
<build>
<finalName>SingleRigidBodyDisplay</finalName>
<directory>build</directory>
<plugins>
<plugin>
<groupId>org.apache.maven.plugins</groupId>
<artifactId>maven-javadoc-plugin</artifactId>
<version>3.1.1</version>
<configuration>
<javadocExecutable>${java.home}/bin/javadoc</javadocExecutable>
<destDir>../../share/doc/trick/java</destDir>
</configuration>
</plugin>
</plugins>
<pluginManagement><!-- lock down plugins versions to avoid using Maven defaults (may be moved to parent pom) -->
<plugins>
<!-- clean lifecycle, see https://maven.apache.org/ref/current/maven-core/lifecycles.html#clean_Lifecycle -->
<plugin>
<artifactId>maven-clean-plugin</artifactId>
<version>3.1.0</version>
</plugin>
<!-- default lifecycle, jar packaging: see https://maven.apache.org/ref/current/maven-core/default-bindings.html#Plugin_bindings_for_jar_packaging -->
<plugin>
<artifactId>maven-resources-plugin</artifactId>
<version>3.0.2</version>
</plugin>
<plugin>
<artifactId>maven-compiler-plugin</artifactId>
<version>3.8.0</version>
<configuration>
<compilerArgs>
<arg>-g</arg>
<arg>-Xlint:unchecked</arg>
<arg>-Xlint:deprecation</arg>
</compilerArgs>
</configuration>
</plugin>
<plugin>
<!-- Build an executable JAR -->
<groupId>org.apache.maven.plugins</groupId>
<artifactId>maven-jar-plugin</artifactId>
<version>3.1.0</version>
<configuration>
<archive>
<manifest>
<addClasspath>true</addClasspath>
<classpathPrefix>lib/</classpathPrefix>
<mainClass>trick.SRBDisplay</mainClass>
</manifest>
</archive>
</configuration>
</plugin>
<plugin>
<artifactId>maven-surefire-plugin</artifactId>
<version>2.22.1</version>
</plugin>
<plugin>
<artifactId>maven-install-plugin</artifactId>
<version>2.5.2</version>
</plugin>
<plugin>
<artifactId>maven-deploy-plugin</artifactId>
<version>2.8.2</version>
</plugin>
<!-- site lifecycle, see https://maven.apache.org/ref/current/maven-core/lifecycles.html#site_Lifecycle -->
<plugin>
<artifactId>maven-site-plugin</artifactId>
<version>3.7.1</version>
</plugin>
<!--
<plugin>
<artifactId>maven-project-info-reports-plugin</artifactId>
<version>3.0.0</version>
</plugin>
-->
</plugins>
</pluginManagement>
</build>
</project>

152
trick_sims/SIM_singlerigidbody/models/singlerigidbodygraphics/src/main/java/trick/matrixops/MatrixOps.java Normal file
View File

@ -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<A.length ; i++) {
R[i] = A[i] + B[i];
}
}
public static void VminusV(double R[], double A[], double B[]) { // Subtracts two matrices together.
if ((A.length != B.length) || (A.length != R.length)) {
System.out.println( "\n MatrixOps::VminusV : Vectors are not the same size.");
return;
}
for (int i=0; i<A.length ; i++) {
R[i] = A[i] - B[i];
}
}
public static void VcrossV(double R[], double A[], double B[]) { // Finds the cross product of two matrices.
if ((R.length != 3) || (A.length != 3) || (B.length != 3)) {
System.out.println( "\n MatrixOps::VcrossV : All vector args must be length 3.");
return;
}
R[0] = A[1] * B[2] - A[2] * B[1];
R[1] = A[2] * B[0] - A[0] * B[2];
R[2] = A[0] * B[1] - A[1] * B[0];
}
public static double VdotV(double A[], double B[]) { // Finds the dot product of two matrices.
if (A.length != B.length) {
System.out.println( "\n MatrixOps::VdotV : Vectors are not the same size.");
return 0.0;
}
double R = 0.0;
for (int i=0; i<A.length ; i++) {
R += A[i] * B[i];
}
return R;
}
public static void Vscale(double R[], double A[], double S) { // Scales a vector (Multiplies by a given number).
if (A.length != R.length) {
System.out.println( "\n MatrixOps::Vscale : Input and output vectors are not the same size.");
return;
}
for (int i=0; i<A.length ; i++) {
R[i] = A[i] * S;
}
}
public static double Vmagnitude (double V[]) { // Returns the magnitude of a vector (Length of a vector).
double S = 0;
for (int i =0; i < V.length ; i++) {
S += V[i]*V[i];
}
return Math.sqrt(S);
}
}

628
trick_sims/SIM_singlerigidbody/models/singlerigidbodygraphics/src/main/java/trick/srbdisplay/SRBDisplay.java Normal file
View File

@ -0,0 +1,628 @@
// This file contains the architecture for the single rigid body simulation's 3D graphics client.
package trick;
// AWT is the abstract window toolkit.
import java.awt.Graphics2D;
import java.awt.Graphics;
import java.awt.Color;
import java.awt.event.MouseEvent;
import java.io.BufferedOutputStream;
import java.io.BufferedReader;
import java.io.DataOutputStream;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.FileReader;
import java.net.Socket;
import java.util.*;
// Swing is a GUI widget toolkit.
import javax.swing.JFrame; // Allows you to manage a top-level window.
import javax.swing.JPanel; // The stuff within the window (controls, buttons, etc.).
import javax.swing.event.MouseInputAdapter;
import trick.matrixOps.MatrixOps;
/**
* @author Aditya Girish
*/
class SingleRigidBodyView extends JPanel { // Main body class of how we view the SRB.
private int screen_half_width;
private int screen_half_height;
private double illumination_vector[]; // World coordinates
private double vantageAzimuth; // Secifies the direction of the celestial light source.
private double vantageElevation;
private double vantageDistance;
private double beta;
private double worldToVantageRotation[][];
private Color bodyLineColor;
private Color bodyFillColor;
private double[] bodyPos;
private double bodyToWorldRotation[][];
private double veh_vrtx_body[][];
private double veh_vrtx_world[][];
private int veh_vrtx_screen[][];
private int veh_edges[][];
private int veh_triangles[][];
private double veh_unit_normals_body[][];
private double veh_unit_normals_world[][];
private double veh_unit_normals_vantage[][];
public SingleRigidBodyView() {
ViewListener viewListener = new ViewListener();
addMouseListener(viewListener);
addMouseMotionListener(viewListener);
// Direction of light.
illumination_vector = new double[] {-0.707, 0.0, -0.707};
// Location of the observer, looking toward the world's origin.
vantageAzimuth = Math.toRadians(45.0);
vantageElevation = Math.toRadians(20.0);
vantageDistance = 22.0;
// Half field of view angle
beta = Math.toRadians(40.0);
worldToVantageRotation = new double[3][3];
setAzElRotation(worldToVantageRotation, vantageAzimuth, vantageElevation);
bodyLineColor = Color.GRAY;
bodyFillColor = new Color(255, 255, 250);
bodyPos = new double[] {0.0, 0.0, 0.0};
bodyToWorldRotation = new double[][] {{1.0, 0.0, 0.0},
{0.0, 1.0, 0.0},
{0.0, 0.0, 1.0}};
// The dodecahedron has 4 "layers" to it. I have labeled them as follows: top (T), upper-middle (UM), lower-middle (LM), and bottom (B)
veh_vrtx_body = new double[][]
{ {-1.37638,0.000000,0.262866}, // 0 - UM
{1.37638,0.000000,-0.262866}, // 1 - LM
{-0.425325,-1.30902,0.262866}, // 2 - UM
{-0.425325,1.30902,0.262866}, // 3 - UM
{1.11352,-0.809017,0.262866}, // 4 - UM
{1.11352,0.809017,0.262866}, // 5 - UM
{-0.262866,-0.809017,1.11352}, // 6 - T
{-0.262866,0.809017,1.11352}, // 7 - T
{-0.688191,-0.5,-1.11352}, // 8 - B
{-0.688191,0.5,-1.11352}, // 9 - B
{0.688191,-0.5,1.11352}, // 10 - T
{0.688191,0.5,1.11352}, // 11 - T
{0.850651,0.000000,-1.11352}, // 12 - B
{-1.11352,-0.809017,-0.262866}, // 13 - LM
{-1.11352,0.809017,-0.262866}, // 14 - LM
{-0.850651,0.000000,1.11352}, // 15 - T
{0.262866,-0.809017,-1.11352}, // 16 - B
{0.262866,0.809017,-1.11352}, // 17 - B
{0.425325,-1.30902,-0.262866}, // 18 - LM
{0.425325,1.30902,-0.262866},}; // 19 - LM
veh_vrtx_world = new double[veh_vrtx_body.length][3];
veh_vrtx_screen = new int[veh_vrtx_body.length][2];
// The edges of the dodecahedron are defined by taking a vertex at the UM or LM of the shape, and defining an edge to the top vertex (for UM) or bottom vertex (for LM), and defining edges to
// each immediate adjacent vertex. The resulting pattern looks like 3 spokes coming out of the original UM or LM vertex.
veh_edges = new int[][]
{ {0, 13}, {0, 14}, {0, 15},
{1, 4}, {1, 5}, {1, 12},
{2, 6}, {2, 13}, {2, 18},
{3, 7}, {3, 14}, {3, 19},
{4, 10}, {4, 18}, {4, 1},
{5, 11}, {5, 19}, {5, 1},
{6, 2}, {6, 15}, {6, 10},
{7, 11}, {7, 15}, {7, 3},
{8, 9}, {8, 13}, {8, 16},
{9, 14}, {9, 8}, {9, 17},
{10, 4}, {10,6}, {10, 11},
{11, 10}, {11, 7}, {11, 5},
{12, 1}, {12, 16}, {12, 17},
{13, 0}, {13, 2}, {13, 8},
{14, 3}, {14, 0}, {14, 9},
{15, 0}, {15, 7}, {15, 6},
{16, 18}, {16, 8}, {16, 12},
{17, 9}, {17, 12}, {17, 19},
{18, 4}, {18, 2}, {18, 16},
{19, 3}, {19, 5}, {19, 17}
};
// The order of the vertices in each of the triangles is significant. The points should follow
// a path counter clock wise about the triangle's normal vector. The position normal side of
// the triangle is the visable side.
veh_triangles = new int[][]
{
{0, 14, 9}, {0, 9, 8}, {0, 8, 13}, // First pentagon using vertices 0, 8, 9, 13, 14
{13, 15, 0}, {13, 6, 15}, {13, 2, 6}, // Second pentagon using vertices 0, 2, 6, 13, 15
{2, 13, 8}, {2, 8, 16}, {2, 16, 18}, // Third pentagon using vertices 2, 8, 13, 16, 18
{18, 6, 2}, {18, 10 ,6}, {18, 4, 10}, // Fourth pentagon using vertices 2, 4, 6, 10, 18
{4, 18, 16}, {4, 16, 12}, {4, 12, 1}, // Fifth pentagon using 1, 4, 12, 16, 18
{1, 10, 4}, {1, 11, 10}, {1, 5, 11}, // Sixth pentagon using vertices 1, 4, 5, 10, 11
{5, 1, 12}, {5, 12, 17}, {5, 17, 19}, // Seventh pentagon using vertices 1, 5, 12, 17, 19
{19, 11, 5}, {19, 7, 11}, {19, 3, 7}, // Eight pentagon using vertices 3, 4, 7, 11, 19
{3, 19, 17}, {3, 17, 9}, {3, 9, 14}, // Ninth pentagon using vertices 3, 9, 14, 17, 19
{14, 7, 3}, {14, 15, 7}, {14, 0, 15}, // Tenth pentagon using vertices 0, 3, 7, 14, 15
{15, 6, 10}, {15, 10, 11}, {15, 11, 7}, // Top pentagon (perpendicular above to xy plane)
{12, 16, 8}, {12, 8, 9}, {12, 9, 17} // Bottom pentagon (perpendicular below to xy plane)
};
// Create the normals for the triangles
veh_unit_normals_body = new double[veh_triangles.length][3];
double v1[] = {0.0, 0.0, 0.0};
double v2[] = {0.0, 0.0, 0.0};
for (int i=0; i < veh_unit_normals_body.length; i++ ) {
MatrixOps.VminusV (v1, veh_vrtx_body[ veh_triangles[i][1] ], veh_vrtx_body[ veh_triangles[i][0] ]);
MatrixOps.VminusV (v2, veh_vrtx_body[ veh_triangles[i][2] ], veh_vrtx_body[ veh_triangles[i][0] ]);
MatrixOps.VcrossV (veh_unit_normals_body[i], v1, v2);
double vmag = MatrixOps.Vmagnitude(veh_unit_normals_body[i]);
MatrixOps.Vscale (veh_unit_normals_body[i], veh_unit_normals_body[i], 1.0/vmag);
}
veh_unit_normals_world = new double[ veh_unit_normals_body.length ][3];
veh_unit_normals_vantage = new double[ veh_unit_normals_body.length ][3];
}
private class ViewListener extends MouseInputAdapter {
private int start_x;
private int start_y;
public void mousePressed(MouseEvent e) {
start_x = e.getX();
start_y = e.getY();
}
public void mouseDragged(MouseEvent e) {
int dx = ( e.getX() - start_x );
int dy = ( start_y - e.getY());
start_x = e.getX();
start_y = e.getY();
mouseVantage( dx, dy);
}
}
public void setVantageRange(double range) {
vantageDistance = range;
}
public void setBodyToWorldRotation( double xx, double xy, double xz,
double yx, double yy, double yz,
double zx, double zy, double zz ) {
bodyToWorldRotation[0][0] = xx;
bodyToWorldRotation[0][1] = xy;
bodyToWorldRotation[0][2] = xz;
bodyToWorldRotation[1][0] = yx;
bodyToWorldRotation[1][1] = yy;
bodyToWorldRotation[1][2] = yz;
bodyToWorldRotation[2][0] = zx;
bodyToWorldRotation[2][1] = zy;
bodyToWorldRotation[2][2] = zz;
}
public void mouseVantage(int dx, int dy) {
vantageAzimuth += (dx * Math.PI) / getWidth();
if (vantageAzimuth > 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<veh_vrtx_body.length ; i++) {
MatrixOps.MtimesV(veh_vrtx_world[i], bodyToWorldRotation, veh_vrtx_body[i]);
MatrixOps.VplusV(veh_vrtx_world[i], veh_vrtx_world[i], bodyPos);
worldToScreenPoint (veh_vrtx_screen[i], veh_vrtx_world[i]);
}
// Draw Solid Model
for (int i=0; i<veh_triangles.length ; i++) {
double LOS_vantage[] = {1.0, 0.0, 0.0};
// Transform the vehicle triangle normals from Body -> 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<veh_edges.length; i++) {
int point0[] = veh_vrtx_screen[ veh_edges[i][0] ];
int point1[] = veh_vrtx_screen[ veh_edges[i][1] ];
g2d.drawLine( point0[0], point0[1], point1[0], point1[1]);
}
// Draw Center of Gravity Point
int CG_screen[] = {0, 0};
int CG_symbol_size = 15;
worldToScreenPoint( CG_screen, bodyPos);
g2d.setPaint( Color.WHITE);
g2d.fillOval(CG_screen[0]-CG_symbol_size/2, CG_screen[1]-CG_symbol_size/2, CG_symbol_size, CG_symbol_size);
g2d.setPaint( Color.BLACK);
g2d.fillArc( CG_screen[0]-CG_symbol_size/2, CG_screen[1]-CG_symbol_size/2, CG_symbol_size, CG_symbol_size, 0, 90 );
g2d.fillArc( CG_screen[0]-CG_symbol_size/2, CG_screen[1]-CG_symbol_size/2, CG_symbol_size, CG_symbol_size, 180, 90);
// Draw World Coordinate Axes
double origin_world[] = {0.0, 0.0, 0.0};
double x_axis_world[] = {9.0, 0.0, 0.0};
drawLineSegmentInWorld(g2d, Color.RED, origin_world, x_axis_world);
drawLabelInWorld(g2d, Color.RED, x_axis_world, "X");
double y_axis_world[] = {0.0, 9.0, 0.0};
drawLineSegmentInWorld(g2d, Color.GREEN, origin_world, y_axis_world);
drawLabelInWorld(g2d, Color.GREEN, y_axis_world, "Y");
double z_axis_world[] = {0.0, 0.0, 9.0};
drawLineSegmentInWorld(g2d, Color.BLUE, origin_world, z_axis_world);
drawLabelInWorld(g2d, Color.BLUE, z_axis_world, "Z");
}
@Override
public void paintComponent( Graphics g) {
super.paintComponent(g);
doDrawing(g);
}
}
public class SRBDisplay extends JFrame {
private SingleRigidBodyView singleRigidBodyView;
private BufferedReader in;
private DataOutputStream out;
public SRBDisplay( SingleRigidBodyView srbv) { // Creates the display window.
singleRigidBodyView = srbv;
add(singleRigidBodyView);
setTitle("SRB Display");
setSize(1597, 987);
setLocationRelativeTo(null);
setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
}
public void connectToServer( String host, int port ) throws IOException { // Connects to the server.
Socket socket = new Socket(host, port);
in = new BufferedReader( new InputStreamReader( socket.getInputStream()));
out = new DataOutputStream(new BufferedOutputStream(socket.getOutputStream()));
}
public void drawSRBView() {
singleRigidBodyView.repaint();
}
private static void printHelpText() {
System.out.println(
"----------------------------------------------------------------------\n"
+ "usage: java jar SingleRigidBodyDisplay.jar <port-number>\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();
}
}
}