mirror of
https://github.com/nasa/trick.git
synced 2024-12-18 12:56:26 +00:00
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 <bnwalla1@scooby.trick.gov> Co-authored-by: AdityaGirish <aditya.girish@nasa.gov>
This commit is contained in:
Aditya Girish
GitHub
parent
aecf6a06fd
commit
0292b52cf7
@ -1,2 +1,14 @@
|
|||||||
TRICK_CFLAGS += -Imodels
|
TRICK_CFLAGS += -Imodels
|
||||||
TRICK_CXXFLAGS += -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
|
||||||
|
|||||||
@ -0,0 +1,6 @@
|
|||||||
|
|
||||||
|
all:
|
||||||
|
mvn package
|
||||||
|
|
||||||
|
clean:
|
||||||
|
rm -rf build
|
||||||
@ -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>
|
||||||
@ -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);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -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();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue
Block a user