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

Initial commit of SIM_splashdown. #1266

Browse Source
This commit is contained in:
John M. Penn 2022-05-20 15:20:45 -05:00
parent 878b895588
commit 7747b53ea1
12 changed files with 1170 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
trick_sims/SIM_splashdown/Modified_data/realtime.py Executable file
View File

@ -0,0 +1,10 @@
trick.real_time_enable()
trick.exec_set_software_frame(0.1)
trick.itimer_enable()
trick.exec_set_enable_freeze(True)
trick.exec_set_freeze_command(True)
simControlPanel = trick.SimControlPanel()
trick.add_external_application(simControlPanel)

37
trick_sims/SIM_splashdown/RUN_test/input.py Normal file
View File

@ -0,0 +1,37 @@
import math
exec(open("Modified_data/realtime.py").read())
crewModule.dyn.position[0] = 0.0;
crewModule.dyn.position[1] = 0.0;
crewModule.dyn.position[2] = 3.0;
crewModule.dyn.mass_vehicle = 9300.0;
# crewModule.dyn.mass_vehicle = 20000.0;
xrot = math.radians(60.0);
crewModule.dyn.R[0][0] = 1.000000;
crewModule.dyn.R[0][1] = 0.000000;
crewModule.dyn.R[0][2] = 0.000000;
crewModule.dyn.R[1][0] = 0.000000;
crewModule.dyn.R[1][1] = math.cos(xrot);
crewModule.dyn.R[1][2] = -math.sin(xrot);
crewModule.dyn.R[2][0] = 0.000000;
crewModule.dyn.R[2][1] = math.sin(xrot);
crewModule.dyn.R[2][2] = math.cos(xrot);
# ==========================================
# Start the CM Display Graphics Client
#==========================================
varServerPort = trick.var_server_get_port();
CMDisplay_path = "models/CrewModuleGraphics/build/CrewModuleDisplay.jar"
if (os.path.isfile(CMDisplay_path)) :
CMDisplay_cmd = "java -jar " \
+ CMDisplay_path \
+ " " + str(varServerPort) + " &" ;
print(CMDisplay_cmd)
os.system( CMDisplay_cmd);
else :
print('==================================================================================')
print('CrewModuleDisplay needs to be built. Please \"cd\" into ../models/CrewModuleGraphics and type \"make\".')
print('==================================================================================')

27
trick_sims/SIM_splashdown/S_define Normal file
View File

@ -0,0 +1,27 @@
/************************************************************
PURPOSE:
( Simulate a Crew Module. )
LIBRARY DEPENDENCIES:
((CrewModule/src/CrewModuleDynamics.o)
(CrewModule/src/CrewModuleShape.o))
**************************************************************************/
#include "sim_objects/default_trick_sys.sm"
##include "CrewModule/include/CrewModuleDynamics.hh"
class CrewModuleSimObject : public Trick::SimObject {
public:
CrewModuleDynamics dyn;
CrewModuleSimObject() {
("default_data") dyn.init_defaults() ;
("derivative") dyn.calc_derivatives() ;
("integration") trick_ret = dyn.calc_state() ;
}
};
CrewModuleSimObject crewModule;
IntegLoop dyn_integloop(0.1) crewModule;
void create_connections() {
dyn_integloop.getIntegrator(Runge_Kutta_4, 18);
}

2
trick_sims/SIM_splashdown/S_overrides.mk Normal file
View File

@ -0,0 +1,2 @@
TRICK_CFLAGS += -Imodels
TRICK_CXXFLAGS += -Imodels

66
trick_sims/SIM_splashdown/models/CrewModule/include/CrewModuleDynamics.hh Normal file
View File

@ -0,0 +1,66 @@
/************************************************************************
PURPOSE: (Simulate a ... .)
LIBRARY DEPENDENCIES:
((CrewModule/src/CrewModuleDynamics.o)((CrewModule/src/CrewModuleShape.o)))
**************************************************************************/
#ifndef CREW_MODULE_DYNAMICS_HH
#define CREW_MODULE_DYNAMICS_HH
#include "../include/CrewModuleShape.hh"
class CrewModuleDynamics {
public:
double R[3][3]; /* (--) Vehicle body to World rotation matrix. */
double Rdot[3][3];
double angular_velocity[3];
double I_body[3][3]; /* (kg*m2) Inertia Tensor in Body frame. */
double I_body_inverse[3][3];
double momentum[3]; /* (kg*m/s) Linear Momentum */
double force_total[3];
double force_gravity[3];
double force_buoyancy[3];
double force_drag[3];
double position[3]; // (m) Position (world coordinates).
double velocity[3]; // (m/s) Velocity (world coordinates).
double mass_vehicle; // (kg) Vehicle mass
double angular_momentum[3]; /* (kg*m2/s) Angular Momentum */
double torque_buoyancy[3];
double torque_drag[3];
double torque_total[3];
double center_of_buoyancy[3];
double volume_displaced;
double mass_displaced;
CrewModuleShape crewModuleShape;
CrewModuleDynamics();
void init_defaults();
void calcVolumeAndCenterOfBuoyancy();
void calcVelocity();
void calcRDot();
void calcAngularVelocity();
void calcTotalTorque();
void calcBuoyancyTorque();
void calcDragTorque();
void calcMassDisplaced();
void calcTotalForce();
void calcBuoyancyForce();
void calcGravityForce();
void calcDragForce();
int calc_derivatives();
int calc_state();
};
#endif

33
trick_sims/SIM_splashdown/models/CrewModule/include/CrewModuleShape.hh Normal file
View File

@ -0,0 +1,33 @@
/************************************************************************
PURPOSE: (Simulate a ... .)
LIBRARY DEPENDENCIES:
((CrewModule/src/CrewModuleShape.o))
**************************************************************************/
#ifndef CREW_MODULE_SHAPE_HH
#define CREW_MODULE_SHAPE_HH
class CrewModuleShape {
public:
double sphere_radius;
double cone_angle;
double sphere_center_base[3];
double cone_point_base[3];
double plane_point_base[3];
double sphere_center_trans[3];
double cone_point_trans[3];
double plane_point_trans[3];
// Work variables
double cone_vector_trans[3];
double plane_vector_trans[3];
CrewModuleShape ();
bool containsPoint(double (&test_point)[3] );
void transformCoordinates( double (&rotation)[3][3], double (&position)[3]);
void transformPoint( double (&rotation)[3][3], double (&translation)[3], double (&in_point)[3], double (&out_point)[3]);
};
#endif

40
trick_sims/SIM_splashdown/models/CrewModule/include/rotation_matrix_macros.h Normal file
View File

@ -0,0 +1,40 @@
#ifndef ROTATION_MATRIX_MACROS_H
#define ROTATION_MATRIX_MACROS_H
#define SET_X_ROTATION_MATRIX( matrix, angle ) { \
matrix[0][0] = 1.0; \
matrix[0][1] = 0.0; \
matrix[0][2] = 0.0; \
matrix[1][0] = 0.0; \
matrix[1][1] = cos((angle)); \
matrix[1][2] = -sin((angle)); \
matrix[2][0] = 0.0; \
matrix[2][1] = sin((angle)); \
matrix[2][2] = cos((angle)); \
}
#define SET_Y_ROTATION_MATRIX( matrix, angle ) { \
matrix[0][0] = cos((angle)); \
matrix[0][1] = 0.0; \
matrix[0][2] = sin((angle)); \
matrix[1][0] = 0.0; \
matrix[1][1] = 1.0; \
matrix[1][2] = 0.0; \
matrix[2][0] = -sin((angle)); \
matrix[2][1] = 0.0; \
matrix[2][2] = cos((angle)); \
}
#define SET_Z_ROTATION_MATRIX( matrix, angle ) { \
matrix[0][0] = cos((angle)); \
matrix[0][1] = -sin((angle)); \
matrix[0][2] = 0.0; \
matrix[1][0] = sin((angle)); \
matrix[1][1] = cos((angle)); \
matrix[1][2] = 0.0; \
matrix[2][0] = 0.0; \
matrix[2][1] = 0.0; \
matrix[2][2] = 1.0; \
}
#endif

179
trick_sims/SIM_splashdown/models/CrewModule/src/CrewModuleDynamics.cpp Normal file
View File

@ -0,0 +1,179 @@
#include <math.h>
#include <stdio.h>
#include "trick/integrator_c_intf.h"
#include "trick/vector_macros.h"
#include "trick/matrix_macros.h"
#include "../include/CrewModuleDynamics.hh"
#include "trick/trick_math_proto.h"
const double acceleration_of_gravity[3] = {0.0, 0.0, -9.81};
const double density_of_water = 1025.0; // kg/m^3 Density of sea water.
CrewModuleDynamics::CrewModuleDynamics() {
init_defaults();
}
void CrewModuleDynamics::init_defaults() {
M_IDENT(R);
M_INIT(Rdot);
mass_vehicle = 9300.0;
momentum[0] = 0.0;
momentum[1] = 0.0;
momentum[2] = 0.0;
position[0] = 0.0;
position[1] = 0.0;
position[2] = 0.0;
angular_momentum[0] = 0.0;
angular_momentum[1] = 0.0;
angular_momentum[2] = 0.0;
crewModuleShape.transformCoordinates(R, position);
double A=2, B=2, C=2;
I_body[0][0] = mass_vehicle * 0.2 * (B*B + C*C);
I_body[0][1] = 0.0;
I_body[0][2] = 0.0;
I_body[1][0] = 0.0;
I_body[1][1] = mass_vehicle * 0.2 * (A*A + C*C);
I_body[1][2] = 0.0;
I_body[2][0] = 0.0;
I_body[2][1] = 0.0;
I_body[2][2] = mass_vehicle * 0.2 * (A*A + B*B);
dm_invert(I_body_inverse, I_body);
}
void CrewModuleDynamics::calcVolumeAndCenterOfBuoyancy() {
double x_start = position[0] - 2.6;
double x_end = position[0] + 2.6;
double y_start = position[1] - 2.6;
double y_end = position[1] + 2.6;
double z_start = position[2] - 2.6;
double z_end = position[2] + 2.6;
double sum_r_dv[3] = {0.0, 0.0, 0.0};
volume_displaced = 0.0;
double P[3];
double dx = 0.1;
double dy = 0.1;
double dz = 0.1;
double dv = dx * dy * dz;
for (double x = x_start; x < x_end ; x+= dx ) {
for (double y = y_start; y < y_end ; y+= dy ) {
for (double z = z_start; z < z_end ; z+= dz ) {
P[0]=x; P[1]=y; P[2]=z;
if ( crewModuleShape.containsPoint(P) ) {
if (P[2] < 0.0) { // Below the water level.
sum_r_dv[0] += x * dv;
sum_r_dv[1] += y * dv;
sum_r_dv[2] += z * dv;
volume_displaced += dv;
}
}
}
}
}
if (volume_displaced > 0.0) {
center_of_buoyancy[0] = sum_r_dv[0] / volume_displaced;
center_of_buoyancy[1] = sum_r_dv[1] / volume_displaced;
center_of_buoyancy[2] = sum_r_dv[2] / volume_displaced;
}
}
void CrewModuleDynamics::calcVelocity() {
double mass_reciprocal = (1.0/mass_vehicle);
V_SCALE(velocity, momentum, mass_reciprocal);
}
void CrewModuleDynamics::calcRDot() {
double omega_skew[3][3];
calcAngularVelocity();
V_SKEW(omega_skew, angular_velocity);
MxM(Rdot, omega_skew, R);
}
void CrewModuleDynamics::calcAngularVelocity() {
double R_transpose[3][3];
double R_I_body_inverse[3][3];
double I_world_inverse[3][3];
MxM(R_I_body_inverse, R, I_body_inverse);
M_TRANS(R_transpose, R);
MxM(I_world_inverse, R_I_body_inverse, R_transpose);
MxV(angular_velocity, I_world_inverse, angular_momentum);
}
void CrewModuleDynamics::calcBuoyancyTorque() {
double lever[3];
V_SUB(lever, center_of_buoyancy, position);
V_CROSS(torque_buoyancy, lever, force_buoyancy);
}
void CrewModuleDynamics::calcDragTorque() {
V_SCALE(torque_drag, angular_velocity, -5000.0);
}
void CrewModuleDynamics::calcTotalTorque() {
calcBuoyancyTorque();
calcDragTorque();
V_ADD(torque_total, torque_buoyancy, torque_drag);
}
void CrewModuleDynamics::calcMassDisplaced() {
mass_displaced = density_of_water * volume_displaced;
}
void CrewModuleDynamics::calcBuoyancyForce() {
calcMassDisplaced();
V_SCALE(force_buoyancy, -acceleration_of_gravity, mass_displaced);
}
void CrewModuleDynamics::calcGravityForce() {
V_SCALE(force_gravity, acceleration_of_gravity, mass_vehicle);
}
void CrewModuleDynamics::calcDragForce() {
V_SCALE(force_drag, velocity, -2000.0);
}
void CrewModuleDynamics::calcTotalForce() {
calcGravityForce();
calcDragForce();
/* force_buoyancy is calculated in calc_derivatives() because two different derivative calculations depend on it. */
V_ADD(force_total, force_gravity, force_buoyancy);
V_ADD(force_total, force_total, force_drag);
}
int CrewModuleDynamics::calc_derivatives() {
crewModuleShape.transformCoordinates(R, position);
calcVolumeAndCenterOfBuoyancy ();
calcBuoyancyForce();
calcTotalForce();
calcVelocity();
calcTotalTorque();
calcRDot();
return 0;
}
int CrewModuleDynamics::calc_state() {
int ipass;
load_state( &momentum[0], &momentum[1], &momentum[2],
&position[0], &position[1], &position[2],
&angular_momentum[0], &angular_momentum[1], &angular_momentum[2],
&R[0][0],&R[0][1],&R[0][2],
&R[1][0],&R[1][1],&R[1][2],
&R[2][0],&R[2][1],&R[2][2],
NULL);
load_deriv( &force_total[0], &force_total[1], &force_total[2],
&velocity[0], &velocity[1], &velocity[2],
&torque_total[0], &torque_total[1], &torque_total[2],
&Rdot[0][0],&Rdot[0][1],&Rdot[0][2],
&Rdot[1][0],&Rdot[1][1],&Rdot[1][2],
&Rdot[2][0],&Rdot[2][1],&Rdot[2][2],
NULL);
ipass = integrate();
unload_state( &momentum[0], &momentum[1], &momentum[2],
&position[0], &position[1], &position[2],
&angular_momentum[0], &angular_momentum[1], &angular_momentum[2],
&R[0][0],&R[0][1],&R[0][2],
&R[1][0],&R[1][1],&R[1][2],
&R[2][0],&R[2][1],&R[2][2],
NULL);
return(ipass);
}

69
trick_sims/SIM_splashdown/models/CrewModule/src/CrewModuleShape.cpp Normal file
View File

@ -0,0 +1,69 @@
#include "../include/CrewModuleShape.hh"
#include "trick/vector_macros.h"
#include "trick/matrix_macros.h"
CrewModuleShape::CrewModuleShape () {
sphere_radius = 6.04;
cone_angle = (M_PI/180.0) * 32.5;
sphere_center_base[0] = 0.00;
sphere_center_base[1] = 0.00;
sphere_center_base[2] = 5.14;
cone_point_base[0] = 0.00;
cone_point_base[1] = 0.00;
cone_point_base[2] = 3.82;
plane_point_base[0] = 0.00;
plane_point_base[1] = 0.00;
plane_point_base[2] = 2.40;
double M[3][3] = {{1.0, 0.0, 0.0},
{0.0, 1.0, 0.0},
{0.0, 0.0, 1.0}};
double P[3] = {0.0, 0.0, 0.0};
transformCoordinates(M, P);
}
bool CrewModuleShape::containsPoint(double (&test_point)[3]) {
double sdiff[3];
V_SUB(sdiff, test_point, sphere_center_trans);
if ( V_MAG(sdiff) <= sphere_radius) { // The point is in the sphere.
double cdiff[3];
V_SUB(cdiff, test_point, cone_point_trans);
double test_angle = acos ( V_DOT(cdiff, cone_vector_trans) / V_MAG(cdiff) );
if ( test_angle < cone_angle ) { // The point is in the cone.
double pdiff[3];
V_SUB(pdiff, test_point, plane_point_trans);
return ( V_DOT(pdiff, plane_vector_trans) > 0.0); // The point is on the correct side of the plane.
}
}
return false;
}
void CrewModuleShape::transformCoordinates( double (&rotation)[3][3], double (&position)[3]) {
transformPoint(rotation, position, sphere_center_base, sphere_center_trans );
transformPoint(rotation, position, cone_point_base, cone_point_trans );
transformPoint(rotation, position, plane_point_base, plane_point_trans );
/*
Calculate these unit vectors so they dont have to be calculated everytime we test a point.
*/
double scale;
V_SUB(cone_vector_trans, position, cone_point_trans);
scale = 1.0/V_MAG(cone_vector_trans);
V_SCALE(cone_vector_trans, cone_vector_trans, scale);
V_SUB(plane_vector_trans, position, plane_point_trans);
scale = 1.0/V_MAG(plane_vector_trans);
V_SCALE(plane_vector_trans, plane_vector_trans, scale);
}
void CrewModuleShape::transformPoint(double (&rotation)[3][3], double (&translation)[3], double (&in_point)[3], double (&out_point)[3] ) {
double work[3];
MxV(work, rotation, in_point);
V_ADD(out_point, work, translation);
}

121
trick_sims/SIM_splashdown/models/CrewModuleGraphics/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>CrewModuleDisplay</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.CMDisplay</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>

477
trick_sims/SIM_splashdown/models/CrewModuleGraphics/src/main/java/trick/cmdisplay/CMDisplay.java Normal file
View File

@ -0,0 +1,477 @@
package trick;
import java.awt.Graphics2D;
import java.awt.Graphics;
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.*;
import javax.swing.JFrame;
import javax.swing.JPanel;
import java.awt.Color;
import java.awt.event.MouseEvent;
import javax.swing.event.MouseInputAdapter;
import trick.matrixOps.MatrixOps;
/**
* @author penn
*/
class CrewModuleView extends JPanel {
private Color waterColor;
private Color vehicleColor;
private Color CBColor;
private double[] vehiclePos;
private double[] centerOfBuoyancy;
private double bodyToWorldRotation[][];
private double vantageAzimuth;
private double vantageElevation;
private double vantageRotation[][];
private double vantageDistance;
private double beta;
private int screen_half_width;
private int screen_half_height;
private double veh_vrtx_body[][];
private double veh_vrtx_world[][];
private int veh_vrtx_screen[][];
private int veh_edges[][];
private double water_vrtx_world[][];
public CrewModuleView() {
ViewListener viewListener = new ViewListener();
addMouseListener(viewListener);
addMouseMotionListener(viewListener);
waterColor = new Color(220,220,250,180);
vehicleColor = new Color(100,100,100);
CBColor = new Color(0,100,255);
vehiclePos = new double[] {0.0, 0.0, 0.0};
centerOfBuoyancy = 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}};
vantageAzimuth = Math.toRadians(45.0);
vantageElevation = Math.toRadians(20.0);
vantageRotation = new double[][] {{1.0, 0.0, 0.0},
{0.0, 1.0, 0.0},
{0.0, 0.0, 1.0}};
updateVantageRotation();
vantageDistance = 15.0;
beta = Math.toRadians(30.0);
veh_vrtx_body = new double[][]
{ { 0.000, 0.000, 2.400}, /* 0 top point*/
{ 0.900, 0.000, 2.400}, /* 1 Upper ring */
{ 0.779, 0.450, 2.400}, /* 2 */
{ 0.450, 0.779, 2.400}, /* 3 */
{ 0.000, 0.900, 2.400}, /* 4 */
{-0.450, 0.779, 2.400}, /* 5 */
{-0.779, 0.450, 2.400}, /* 6 */
{-0.900, 0.000, 2.400}, /* 7 */
{-0.779,-0.450, 2.400}, /* 8 */
{-0.450,-0.779, 2.400}, /* 9 */
{ 0.000,-0.900, 2.400}, /* 10 */
{ 0.450,-0.779, 2.400}, /* 11 */
{ 0.779,-0.450, 2.400}, /* 12 */
{ 2.500, 0.000,-0.100}, /* 13 Middle ring */
{ 2.166, 1.250,-0.100}, /* 14 */
{ 1.250, 2.166,-0.100}, /* 15 */
{ 0.000, 2.500,-0.100}, /* 16 */
{-1.250, 2.166,-0.100}, /* 17 */
{-2.166, 1.250,-0.100}, /* 18 */
{-2.500, 0.000,-0.100}, /* 19 */
{-2.166,-1.250,-0.100}, /* 20 */
{-1.250,-2.166,-0.100}, /* 21 */
{ 0.000,-2.500,-0.100}, /* 22 */
{ 1.250,-2.166,-0.100}, /* 23 */
{ 2.166,-1.250,-0.100}, /* 24 */
{ 2.500, 0.000,-0.300}, /* 25 Lower ring */
{ 2.166, 1.250,-0.300}, /* 26 */
{ 1.250, 2.166,-0.300}, /* 27 */
{ 0.000, 2.500,-0.300}, /* 28 */
{-1.250, 2.166,-0.300}, /* 29 */
{-2.166, 1.250,-0.300}, /* 30 */
{-2.500, 0.000,-0.300}, /* 31 */
{-2.166,-1.250,-0.300}, /* 32 */
{-1.250,-2.166,-0.300}, /* 33 */
{ 0.000,-2.500,-0.300}, /* 34 */
{ 1.250,-2.166,-0.300}, /* 35 */
{ 2.166,-1.250,-0.300}, /* 36 */
{ 0.000, 0.000,-0.900} /* 37 bottom point */
};
veh_vrtx_world = new double[veh_vrtx_body.length][3];
veh_vrtx_screen = new int[veh_vrtx_body.length][2];
veh_edges = new int[][]
{ /* connect top-center and upper ring */
{ 0, 1},{ 0, 2},{ 0, 3},{ 0, 4},{ 0, 5},{ 0, 6},{ 0, 7},{ 0, 8},{ 0, 9},{ 0,10},{ 0,11},{ 0,12},
/* connect upper ring points */
{ 1, 2},{ 2, 3},{ 3, 4},{ 4, 5},{ 5, 6},{ 6, 7},{ 7, 8},{ 8, 9},{ 9,10},{10,11},{11,12},{12, 1},
/* connect upper and middle rings */
{ 1,13},{ 2,14},{ 3,15},{ 4,16},{ 5,17},{ 6,18},{ 7,19},{ 8,20},{ 9,21},{10,22},{11,23},{12,24},
/* connect middle ring points */
{13,14},{14,15},{15,16},{16,17},{17,18},{18,19},{19,20},{20,21},{21,22},{22,23},{23,24},{24,13},
/* connect middle and lower rings */
{13,25},{14,26},{15,27},{16,28},{17,29},{18,30},{19,31},{20,32},{21,33},{22,34},{23,35},{24,36},
/* connect lower ring points */
{25,26},{26,27},{27,28},{28,29},{29,30},{30,31},{31,32},{32,33},{33,34},{34,35},{35,36},{36,25},
/* connect lower points to bottom-center */
{25,37},{26,37},{27,37},{28,37},{29,37},{30,37},{31,37},{32,37},{33,37},{34,37},{35,37},{36,37}
};
water_vrtx_world = new double[][]
{
{ 5.000, 5.000, 0.000},
{ 5.000,-5.000, 0.000},
{-5.000,-5.000, 0.000},
{-5.000, 5.000, 0.000}
};
}
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);
updateVantageRotation();
repaint();
}
public void updateVantageRotation() {
double Rotation_about_Y[][] = {
{ Math.cos(vantageElevation), 0.0, Math.sin(vantageElevation)},
{ 0.0, 1.0, 0.0},
{-Math.sin(vantageElevation), 0.0, Math.cos(vantageElevation)}
};
double Rotation_about_Z[][] = {
{Math.cos(vantageAzimuth), -Math.sin(vantageAzimuth), 0.0},
{Math.sin(vantageAzimuth), Math.cos(vantageAzimuth), 0.0},
{ 0.0, 0.0, 1.0}
};
MatrixOps.MtimesM( vantageRotation, Rotation_about_Y, Rotation_about_Z);
}
public void worldToScreenPoint( int result[], double X_world[]) {
// X_world to X_vantage
double x_vantage[] = new double[3];
MatrixOps.MtimesV(x_vantage, vantageRotation, X_world);
// X_vantage to screen
double perspective_scale = screen_half_width/(Math.tan(beta)*(vantageDistance-x_vantage[0]));
result[0] = (int)(perspective_scale * x_vantage[1] + screen_half_width);
result[1] = (int)(screen_half_height - perspective_scale * x_vantage[2]);
}
public void setVehPos( double x, double y, double z) {
vehiclePos[0] = x;
vehiclePos[1] = y;
vehiclePos[2] = z;
}
public void setCB(double CBx, double CBy, double CBz) {
centerOfBuoyancy[0] = CBx;
centerOfBuoyancy[1] = CBy;
centerOfBuoyancy[2] = CBz;
}
private void fillCenteredCircle(Graphics2D g, int x, int y, int r) {
x = x-(r/2);
y = y-(r/2);
g.fillOval(x,y,r,r);
}
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);
// ======================
// WATER
// ======================
g2d.setPaint( waterColor);
int pt[] = {0, 0};
int wx[] = {0, 0, 0, 0};
int wy[] = {0, 0, 0, 0};
for (int i=0; i<water_vrtx_world.length ; i++) {
worldToScreenPoint( pt, water_vrtx_world[i]);
wx[i] = pt[0];
wy[i] = pt[1];
}
g2d.fillPolygon(wx, wy, 4);
g2d.setPaint( Color.BLUE);
g2d.drawPolygon(wx, wy, 4);
// =========================
// Center of Buoyancy Point
// =========================
g2d.setPaint( CBColor);
int CB_screen[] = {0, 0};
worldToScreenPoint( CB_screen, centerOfBuoyancy);
fillCenteredCircle(g2d, CB_screen[0], CB_screen[1], 15);
g2d.drawString("CB", CB_screen[0]+10, CB_screen[1]+5);
// =========================
// Center of Gravity Point
// =========================
int CG_screen[] = {0, 0};
worldToScreenPoint( CG_screen, vehiclePos);
int CG_symbol_size = 15;
g2d.setPaint( Color.WHITE);
fillCenteredCircle(g2d, CG_screen[0], CG_screen[1], 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);
// ======================
// VEHICLE
// ======================
g2d.setPaint( vehicleColor);
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], vehiclePos);
worldToScreenPoint (veh_vrtx_screen[i], veh_vrtx_world[i]);
}
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]);
}
// ======================
// WORLD COORDINATE AXES
// ======================
double origin_world[] = {0.0, 0.0, 0.0};
int origin_screen[] = {0, 0};
worldToScreenPoint( origin_screen, origin_world);
g2d.setPaint(Color.RED);
double x_axis_world[] = {5.0, 0.0, 0.0};
int x_axis_screen[] = {0, 0};
worldToScreenPoint( x_axis_screen, x_axis_world);
g2d.drawLine( origin_screen[0], origin_screen[1], x_axis_screen[0], x_axis_screen[1]);
g2d.drawString ( "X", x_axis_screen[0],x_axis_screen[1]);
g2d.setPaint(Color.GREEN);
double y_axis_world[] = {0.0, 5.0, 0.0};
int y_axis_screen[] = {0, 0};
worldToScreenPoint( y_axis_screen, y_axis_world);
g2d.drawLine( origin_screen[0], origin_screen[1], y_axis_screen[0], y_axis_screen[1]);
g2d.drawString ( "Y", y_axis_screen[0],y_axis_screen[1]);
g2d.setPaint(Color.BLUE);
double z_axis_world[] = {0.0, 0.0, 5.0};
int z_axis_screen[] = {0, 0};
worldToScreenPoint( z_axis_screen, z_axis_world);
g2d.drawLine( origin_screen[0], origin_screen[1], z_axis_screen[0], z_axis_screen[1]);
g2d.drawString ( "Z", z_axis_screen[0],z_axis_screen[1]);
}
@Override
public void paintComponent( Graphics g) {
super.paintComponent(g);
doDrawing(g);
}
}
public class CMDisplay extends JFrame {
private CrewModuleView crewModuleView;
private BufferedReader in;
private DataOutputStream out;
public CMDisplay( CrewModuleView cmv) {
crewModuleView = cmv;
add( crewModuleView);
setTitle("CM Display");
setSize(1597, 987);
setLocationRelativeTo(null);
setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
}
public void connectToServer( String host, int port ) throws IOException {
Socket socket = new Socket(host, port);
in = new BufferedReader( new InputStreamReader( socket.getInputStream()));
out = new DataOutputStream(new BufferedOutputStream(socket.getOutputStream()));
}
public void drawCrewModuleView() {
crewModuleView.repaint();
}
private static void printHelpText() {
System.out.println(
"----------------------------------------------------------------------\n"
+ "usage: java jar CMDisplay.jar <port-number>\n"
+ "----------------------------------------------------------------------\n"
);
}
public static void main(String[] args) throws IOException {
String host = "localHost";
int port = 0;
String vehicleImageFile = 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);
}
CrewModuleView crewModuleView = new CrewModuleView();
CMDisplay sd = new CMDisplay(crewModuleView);
sd.setVisible(true);
double vehX = 0.0;
double vehY = 0.0;
double vehZ = 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 CBx = 0.0;
double CBy = 0.0;
double CBz = 0.0;
System.out.println("Connecting to: " + host + ":" + port);
sd.connectToServer(host, port);
sd.out.writeBytes("trick.var_set_client_tag(\"CMDisplay\") \n" +
"trick.var_pause() \n" +
"trick.var_add(\"crewModule.dyn.position[0]\") \n" +
"trick.var_add(\"crewModule.dyn.position[1]\") \n" +
"trick.var_add(\"crewModule.dyn.position[2]\") \n" +
"trick.var_add(\"crewModule.dyn.R[0][0]\") \n" +
"trick.var_add(\"crewModule.dyn.R[0][1]\") \n" +
"trick.var_add(\"crewModule.dyn.R[0][2]\") \n" +
"trick.var_add(\"crewModule.dyn.R[1][0]\") \n" +
"trick.var_add(\"crewModule.dyn.R[1][1]\") \n" +
"trick.var_add(\"crewModule.dyn.R[1][2]\") \n" +
"trick.var_add(\"crewModule.dyn.R[2][0]\") \n" +
"trick.var_add(\"crewModule.dyn.R[2][1]\") \n" +
"trick.var_add(\"crewModule.dyn.R[2][2]\") \n" +
"trick.var_add(\"crewModule.dyn.center_of_buoyancy[0]\") \n" +
"trick.var_add(\"crewModule.dyn.center_of_buoyancy[1]\") \n" +
"trick.var_add(\"crewModule.dyn.center_of_buoyancy[2]\") \n" +
"trick.var_ascii() \n" +
"trick.var_cycle(0.1) \n" +
"trick.var_unpause()\n" );
sd.out.flush();
sd.drawCrewModuleView();
Boolean go = true;
while (go) {
String field[];
try {
String line;
line = sd.in.readLine();
field = line.split("\t");
vehX = Double.parseDouble( field[1] );
vehY = Double.parseDouble( field[2] );
vehZ = Double.parseDouble( field[3] );
Rxx = Double.parseDouble( field[4] );
Rxy = Double.parseDouble( field[5] );
Rxz = Double.parseDouble( field[6] );
Ryx = Double.parseDouble( field[7] );
Ryy = Double.parseDouble( field[8] );
Ryz = Double.parseDouble( field[9] );
Rzx = Double.parseDouble( field[10] );
Rzy = Double.parseDouble( field[11] );
Rzz = Double.parseDouble( field[12] );
CBx = Double.parseDouble( field[13] );
CBy = Double.parseDouble( field[14] );
CBz = Double.parseDouble( field[15] );
// Set the Vehicle position
crewModuleView.setVehPos(vehX, vehY, vehZ);
crewModuleView.setBodyToWorldRotation( Rxx, Rxy, Rxz,
Ryx, Ryy, Ryz,
Rzx, Rzy, Rzz );
crewModuleView.setCB(CBx, CBy, CBz);
} catch (IOException | NullPointerException e ) {
go = false;
}
sd.drawCrewModuleView();
}
}
}

109
trick_sims/SIM_splashdown/models/CrewModuleGraphics/src/main/java/trick/matrixops/MatrixOps.java Normal file
View File

@ -0,0 +1,109 @@
package trick.matrixOps;
import java.io.*;
public class MatrixOps {
public static void printMatrix(double M[][]) {
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[]) {
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[]) {
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[][]) {
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) {
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)) {
System.out.println( "\n Result matrix is wrong size.");
return;
}
for (int i = 0; i < A_rows; i++) {
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[]) {
int M_rows = M.length;
int M_cols = M[0].length;
int V_rows = V.length;
if (M_cols != V_rows) {
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) {
System.out.println( "\n Result vector is wrong size.");
return;
}
for (int i =0; i < M_rows ; i++) {
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[]) {
int A_rows = A.length;
int B_rows = B.length;
int R_rows = R.length;
if ((A_rows != B_rows) || (A_rows != R_rows)) {
System.out.println( "\n vector are wrong size.");
}
for (int i=0; i<A_rows ; i++) {
R[i] = A[i] + B[i];
}
}
public static double vector_magnitude (double V[]) {
double S = 0;
for (int i =0; i < V.length ; i++) {
S += V[i]*V[i];
}
return Math.sqrt(S);
}
}