diff --git a/trick_sims/SIM_splashdown/Modified_data/realtime.py b/trick_sims/SIM_splashdown/Modified_data/realtime.py new file mode 100755 index 00000000..acf31873 --- /dev/null +++ b/trick_sims/SIM_splashdown/Modified_data/realtime.py @@ -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) diff --git a/trick_sims/SIM_splashdown/RUN_test/input.py b/trick_sims/SIM_splashdown/RUN_test/input.py new file mode 100644 index 00000000..8ce10cfe --- /dev/null +++ b/trick_sims/SIM_splashdown/RUN_test/input.py @@ -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('==================================================================================') diff --git a/trick_sims/SIM_splashdown/S_define b/trick_sims/SIM_splashdown/S_define new file mode 100644 index 00000000..55d9ae38 --- /dev/null +++ b/trick_sims/SIM_splashdown/S_define @@ -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); +} diff --git a/trick_sims/SIM_splashdown/S_overrides.mk b/trick_sims/SIM_splashdown/S_overrides.mk new file mode 100644 index 00000000..e1f6cccd --- /dev/null +++ b/trick_sims/SIM_splashdown/S_overrides.mk @@ -0,0 +1,2 @@ +TRICK_CFLAGS += -Imodels +TRICK_CXXFLAGS += -Imodels diff --git a/trick_sims/SIM_splashdown/models/CrewModule/include/CrewModuleDynamics.hh b/trick_sims/SIM_splashdown/models/CrewModule/include/CrewModuleDynamics.hh new file mode 100644 index 00000000..a816f44e --- /dev/null +++ b/trick_sims/SIM_splashdown/models/CrewModule/include/CrewModuleDynamics.hh @@ -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 diff --git a/trick_sims/SIM_splashdown/models/CrewModule/include/CrewModuleShape.hh b/trick_sims/SIM_splashdown/models/CrewModule/include/CrewModuleShape.hh new file mode 100644 index 00000000..d1c93be8 --- /dev/null +++ b/trick_sims/SIM_splashdown/models/CrewModule/include/CrewModuleShape.hh @@ -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 diff --git a/trick_sims/SIM_splashdown/models/CrewModule/include/rotation_matrix_macros.h b/trick_sims/SIM_splashdown/models/CrewModule/include/rotation_matrix_macros.h new file mode 100644 index 00000000..44ba036a --- /dev/null +++ b/trick_sims/SIM_splashdown/models/CrewModule/include/rotation_matrix_macros.h @@ -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 diff --git a/trick_sims/SIM_splashdown/models/CrewModule/src/CrewModuleDynamics.cpp b/trick_sims/SIM_splashdown/models/CrewModule/src/CrewModuleDynamics.cpp new file mode 100644 index 00000000..00744b4b --- /dev/null +++ b/trick_sims/SIM_splashdown/models/CrewModule/src/CrewModuleDynamics.cpp @@ -0,0 +1,179 @@ +#include +#include +#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); + +} diff --git a/trick_sims/SIM_splashdown/models/CrewModule/src/CrewModuleShape.cpp b/trick_sims/SIM_splashdown/models/CrewModule/src/CrewModuleShape.cpp new file mode 100644 index 00000000..9d0f2c06 --- /dev/null +++ b/trick_sims/SIM_splashdown/models/CrewModule/src/CrewModuleShape.cpp @@ -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); +} diff --git a/trick_sims/SIM_splashdown/models/CrewModuleGraphics/pom.xml b/trick_sims/SIM_splashdown/models/CrewModuleGraphics/pom.xml new file mode 100644 index 00000000..2b994f5e --- /dev/null +++ b/trick_sims/SIM_splashdown/models/CrewModuleGraphics/pom.xml @@ -0,0 +1,121 @@ + + + + 4.0.0 + + trick-java + trick-java + 23.0.0-beta + + trick-java + + https://github.com/nasa/trick + + + UTF-8 + 1.8 + 1.8 + + + + + junit + junit + 4.13.1 + test + + + + + + CrewModuleDisplay + + build + + + org.apache.maven.plugins + maven-javadoc-plugin + 3.1.1 + + ${java.home}/bin/javadoc + ../../share/doc/trick/java + + + + + + + + + + maven-clean-plugin + 3.1.0 + + + + + maven-resources-plugin + 3.0.2 + + + + maven-compiler-plugin + 3.8.0 + + + -g + -Xlint:unchecked + -Xlint:deprecation + + + + + + + org.apache.maven.plugins + maven-jar-plugin + 3.1.0 + + + + true + lib/ + trick.CMDisplay + + + + + + + maven-surefire-plugin + 2.22.1 + + + + maven-install-plugin + 2.5.2 + + + + maven-deploy-plugin + 2.8.2 + + + + + maven-site-plugin + 3.7.1 + + + + + + + + diff --git a/trick_sims/SIM_splashdown/models/CrewModuleGraphics/src/main/java/trick/cmdisplay/CMDisplay.java b/trick_sims/SIM_splashdown/models/CrewModuleGraphics/src/main/java/trick/cmdisplay/CMDisplay.java new file mode 100644 index 00000000..a518ba20 --- /dev/null +++ b/trick_sims/SIM_splashdown/models/CrewModuleGraphics/src/main/java/trick/cmdisplay/CMDisplay.java @@ -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\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(); + } + } +} diff --git a/trick_sims/SIM_splashdown/models/CrewModuleGraphics/src/main/java/trick/matrixops/MatrixOps.java b/trick_sims/SIM_splashdown/models/CrewModuleGraphics/src/main/java/trick/matrixops/MatrixOps.java new file mode 100644 index 00000000..49135da1 --- /dev/null +++ b/trick_sims/SIM_splashdown/models/CrewModuleGraphics/src/main/java/trick/matrixops/MatrixOps.java @@ -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