diff --git a/trick_sims/SIM_splashdown/models/CrewModule/src/CrewModuleDynamics.cpp b/trick_sims/SIM_splashdown/models/CrewModule/src/CrewModuleDynamics.cpp index 00744b4b..8e54b3d4 100644 --- a/trick_sims/SIM_splashdown/models/CrewModule/src/CrewModuleDynamics.cpp +++ b/trick_sims/SIM_splashdown/models/CrewModule/src/CrewModuleDynamics.cpp @@ -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); - } 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 index a518ba20..ef9a479a 100644 --- 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 @@ -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