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

Tidy up dynamics code and GUI client. #1266

Browse Source
This commit is contained in:
John M. Penn 2022-05-21 23:56:25 -05:00
parent 3171c71cb7
commit 03dd6bd313
2 changed files with 55 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
trick_sims/SIM_splashdown/models/CrewModule/src/CrewModuleDynamics.cpp
View File

@ -27,16 +27,16 @@ void CrewModuleDynamics::init_defaults() {
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);
double A=1, B=1, C=1;
I_body[0][0] = mass_vehicle * (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][1] = mass_vehicle * (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);
I_body[2][2] = mass_vehicle * (A*A + B*B);
dm_invert(I_body_inverse, I_body);
}
@ -73,6 +73,10 @@ void CrewModuleDynamics::calcVolumeAndCenterOfBuoyancy() {
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;
} else {
center_of_buoyancy[0] = position[0];
center_of_buoyancy[1] = position[1];
center_of_buoyancy[2] = position[2];
}
}
@ -99,9 +103,9 @@ void CrewModuleDynamics::calcAngularVelocity() {
}
void CrewModuleDynamics::calcBuoyancyTorque() {
double lever[3];
V_SUB(lever, center_of_buoyancy, position);
V_CROSS(torque_buoyancy, lever, force_buoyancy);
double lever[3];
V_SUB(lever, center_of_buoyancy, position);
V_CROSS(torque_buoyancy, lever, force_buoyancy);
}
void CrewModuleDynamics::calcDragTorque() {
@ -128,7 +132,7 @@ void CrewModuleDynamics::calcGravityForce() {
}
void CrewModuleDynamics::calcDragForce() {
V_SCALE(force_drag, velocity, -2000.0);
V_SCALE(force_drag, velocity, -2000.0);
}
void CrewModuleDynamics::calcTotalForce() {
@ -140,40 +144,44 @@ void CrewModuleDynamics::calcTotalForce() {
}
int CrewModuleDynamics::calc_derivatives() {
crewModuleShape.transformCoordinates(R, position);
calcVolumeAndCenterOfBuoyancy ();
calcBuoyancyForce();
calcTotalForce();
calcVelocity();
calcTotalTorque();
calcRDot();
return 0;
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);
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);
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);
int axis = 0;
dm_orthonormal(R, &axis); // Square up the rotation matrix.
return(ipass);
}

14
trick_sims/SIM_splashdown/models/CrewModuleGraphics/src/main/java/trick/cmdisplay/CMDisplay.java
View File

@ -26,7 +26,7 @@ class CrewModuleView extends JPanel {
private Color waterColor;
private Color vehicleColor;
private Color CBColor;
// private Color CBColor;
private double[] vehiclePos;
private double[] centerOfBuoyancy;
private double bodyToWorldRotation[][];
@ -51,7 +51,7 @@ class CrewModuleView extends JPanel {
waterColor = new Color(220,220,250,180);
vehicleColor = new Color(100,100,100);
CBColor = new Color(0,100,255);
// CBColor = new Color(0,100,255);
vehiclePos = new double[] {0.0, 0.0, 0.0};
centerOfBuoyancy = new double[] {0.0, 0.0, 0.0};
@ -253,11 +253,15 @@ class CrewModuleView extends JPanel {
// =========================
// 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);
int CB_symbol_size = 15;
g2d.setPaint( Color.WHITE);
fillCenteredCircle(g2d, CB_screen[0], CB_screen[1], CB_symbol_size);
g2d.setPaint( Color.BLUE);
g2d.fillArc( CB_screen[0]-CB_symbol_size/2, CB_screen[1]-CB_symbol_size/2, CB_symbol_size, CB_symbol_size, 0, 90 );
g2d.fillArc( CB_screen[0]-CB_symbol_size/2, CB_screen[1]-CB_symbol_size/2, CB_symbol_size, CB_symbol_size, 180, 90);
// g2d.drawString("CB", CB_screen[0]+10, CB_screen[1]+5);
// =========================
// Center of Gravity Point