git.gag.com Git - debian/openrocket/blob - core/src/net/sf/openrocket/simulation/RK4SimulationStepper.java

   1 package net.sf.openrocket.simulation;
   2
   3 import java.util.Arrays;
   4 import java.util.Random;
   5
   6 import net.sf.openrocket.aerodynamics.AerodynamicForces;
   7 import net.sf.openrocket.aerodynamics.FlightConditions;
   8 import net.sf.openrocket.aerodynamics.WarningSet;
   9 import net.sf.openrocket.l10n.Translator;
  10 import net.sf.openrocket.logging.LogHelper;
  11 import net.sf.openrocket.models.atmosphere.AtmosphericConditions;
  12 import net.sf.openrocket.simulation.exception.SimulationCalculationException;
  13 import net.sf.openrocket.simulation.exception.SimulationException;
  14 import net.sf.openrocket.simulation.listeners.SimulationListenerHelper;
  15 import net.sf.openrocket.startup.Application;
  16 import net.sf.openrocket.util.Coordinate;
  17 import net.sf.openrocket.util.GeodeticComputationStrategy;
  18 import net.sf.openrocket.util.MathUtil;
  19 import net.sf.openrocket.util.Quaternion;
  20 import net.sf.openrocket.util.Rotation2D;
  21 import net.sf.openrocket.util.WorldCoordinate;
  22
  23 public class RK4SimulationStepper extends AbstractSimulationStepper {
  24
  25         private static final LogHelper log = Application.getLogger();
  26         private static final Translator trans = Application.getTranslator();
  27
  28
  29         /** Random value with which to XOR the random seed value */
  30         private static final int SEED_RANDOMIZATION = 0x23E3A01F;
  31
  32
  33         /**
  34          * A recommended reasonably accurate time step.
  35          */
  36         public static final double RECOMMENDED_TIME_STEP = 0.05;
  37
  38         /**
  39          * A recommended maximum angle step value.
  40          */
  41         public static final double RECOMMENDED_ANGLE_STEP = 3 * Math.PI / 180;
  42
  43         /**
  44          * A random amount that is added to pitch and yaw coefficients, plus or minus.
  45          */
  46         public static final double PITCH_YAW_RANDOM = 0.0005;
  47
  48         /**
  49          * Maximum roll step allowed.  This is selected as an uneven division of the full
  50          * circle so that the simulation will sample the most wind directions
  51          */
  52         private static final double MAX_ROLL_STEP_ANGLE = 2 * 28.32 * Math.PI / 180;
  53         //      private static final double MAX_ROLL_STEP_ANGLE = 8.32 * Math.PI/180;
  54
  55         private static final double MAX_ROLL_RATE_CHANGE = 2 * Math.PI / 180;
  56         private static final double MAX_PITCH_CHANGE = 4 * Math.PI / 180;
  57
  58         private static final double MIN_TIME_STEP = 0.001;
  59
  60
  61         private Random random;
  62
  63
  64
  65
  66         @Override
  67         public RK4SimulationStatus initialize(SimulationStatus original) {
  68
  69                 RK4SimulationStatus status = new RK4SimulationStatus();
  70
  71                 status.copyFrom(original);
  72
  73                 SimulationConditions sim = original.getSimulationConditions();
  74
  75                 status.setLaunchRodDirection(new Coordinate(
  76                                 Math.sin(sim.getLaunchRodAngle()) * Math.cos(sim.getLaunchRodDirection()),
  77                                 Math.sin(sim.getLaunchRodAngle()) * Math.sin(sim.getLaunchRodDirection()),
  78                                 Math.cos(sim.getLaunchRodAngle())
  79                                 ));
  80
  81                 this.random = new Random(original.getSimulationConditions().getRandomSeed() ^ SEED_RANDOMIZATION);
  82
  83                 return status;
  84         }
  85
  86
  87
  88
  89         @Override
  90         public void step(SimulationStatus simulationStatus, double maxTimeStep) throws SimulationException {
  91
  92                 RK4SimulationStatus status = (RK4SimulationStatus) simulationStatus;
  93                 DataStore store = new DataStore();
  94
  95                 ////////  Perform RK4 integration:  ////////
  96
  97                 RK4SimulationStatus status2;
  98                 RK4Parameters k1, k2, k3, k4;
  99
 100                 /*
 101                  * Start with previous time step which is used to compute the initial thrust estimate.
 102                  * Don't make it longer than maxTimeStep, but at least MIN_TIME_STEP.
 103                  */
 104                 store.timestep = status.getPreviousTimeStep();
 105                 store.timestep = MathUtil.max(MathUtil.min(store.timestep, maxTimeStep), MIN_TIME_STEP);
 106                 checkNaN(store.timestep);
 107
 108                 /*
 109                  * Compute the initial thrust estimate.  This is used for the first time step computation.
 110                  */
 111                 store.thrustForce = calculateThrust(status, store.timestep, status.getPreviousAcceleration(),
 112                                 status.getPreviousAtmosphericConditions(), false);
 113
 114
 115                 /*
 116                  * Perform RK4 integration.  Decide the time step length after the first step.
 117                  */
 118
 119                 //// First position, k1 = f(t, y)
 120
 121                 k1 = computeParameters(status, store);
 122
 123                 /*
 124                  * Select the actual time step to use.  It is the minimum of the following:
 125                  *  dt[0]:  the user-specified time step (or 1/5th of it if still on the launch rod)
 126                  *  dt[1]:  the value of maxTimeStep
 127                  *  dt[2]:  the maximum pitch step angle limit
 128                  *  dt[3]:  the maximum roll step angle limit
 129                  *  dt[4]:  the maximum roll rate change limit
 130                  *  dt[5]:  the maximum pitch change limit
 131                  *  dt[6]:  1/10th of the launch rod length if still on the launch rod
 132                  *  dt[7]:  1.50 times the previous time step
 133                  *
 134                  * The limits #5 and #6 are required since near the steady-state roll rate the roll rate
 135                  * may oscillate significantly even between the sub-steps of the RK4 integration.
 136                  *
 137                  * The step is still at least 1/20th of the user-selected time step.
 138                  */
 139                 double[] dt = new double[8];
 140                 Arrays.fill(dt, Double.MAX_VALUE);
 141
 142                 dt[0] = status.getSimulationConditions().getTimeStep();
 143                 dt[1] = maxTimeStep;
 144                 dt[2] = status.getSimulationConditions().getMaximumAngleStep() / store.lateralPitchRate;
 145                 dt[3] = Math.abs(MAX_ROLL_STEP_ANGLE / store.flightConditions.getRollRate());
 146                 dt[4] = Math.abs(MAX_ROLL_RATE_CHANGE / store.rollAcceleration);
 147                 dt[5] = Math.abs(MAX_PITCH_CHANGE / store.lateralPitchAcceleration);
 148                 if (!status.isLaunchRodCleared()) {
 149                         dt[0] /= 5.0;
 150                         dt[6] = status.getSimulationConditions().getLaunchRodLength() / k1.v.length() / 10;
 151                 }
 152                 dt[7] = 1.5 * status.getPreviousTimeStep();
 153
 154                 store.timestep = Double.MAX_VALUE;
 155                 int limitingValue = -1;
 156                 for (int i = 0; i < dt.length; i++) {
 157                         if (dt[i] < store.timestep) {
 158                                 store.timestep = dt[i];
 159                                 limitingValue = i;
 160                         }
 161                 }
 162
 163                 double minTimeStep = status.getSimulationConditions().getTimeStep() / 20;
 164                 if (store.timestep < minTimeStep) {
 165                         log.verbose("Too small time step " + store.timestep + " (limiting factor " + limitingValue + "), using " +
 166                                         minTimeStep + " instead.");
 167                         store.timestep = minTimeStep;
 168                 } else {
 169                         log.verbose("Selected time step " + store.timestep + " (limiting factor " + limitingValue + ")");
 170                 }
 171                 checkNaN(store.timestep);
 172
 173                 /*
 174                  * Compute the correct thrust for this time step.  If the original thrust estimate differs more
 175                  * than 10% from the true value then recompute the RK4 step 1.  The 10% error in step 1 is
 176                  * diminished by it affecting only 1/6th of the total, so it's an acceptable error.
 177                  */
 178                 double thrustEstimate = store.thrustForce;
 179                 store.thrustForce = calculateThrust(status, store.timestep, store.longitudinalAcceleration,
 180                                 store.atmosphericConditions, true);
 181                 double thrustDiff = Math.abs(store.thrustForce - thrustEstimate);
 182                 // Log if difference over 1%, recompute if over 10%
 183                 if (thrustDiff > 0.01 * thrustEstimate) {
 184                         if (thrustDiff > 0.1 * thrustEstimate + 0.001) {
 185                                 log.debug("Thrust estimate differs from correct value by " +
 186                                                 (Math.rint(1000 * (thrustDiff + 0.000001) / thrustEstimate) / 10.0) + "%," +
 187                                                 " estimate=" + thrustEstimate +
 188                                                 " correct=" + store.thrustForce +
 189                                                 " timestep=" + store.timestep +
 190                                                 ", recomputing k1 parameters");
 191                                 k1 = computeParameters(status, store);
 192                         } else {
 193                                 log.verbose("Thrust estimate differs from correct value by " +
 194                                                 (Math.rint(1000 * (thrustDiff + 0.000001) / thrustEstimate) / 10.0) + "%," +
 195                                                 " estimate=" + thrustEstimate +
 196                                                 " correct=" + store.thrustForce +
 197                                                 " timestep=" + store.timestep +
 198                                                 ", error acceptable");
 199                         }
 200                 }
 201
 202                 // Store data
 203                 // TODO: MEDIUM: Store acceleration etc of entire RK4 step, store should be cloned or something...
 204                 storeData(status, store);
 205
 206
 207                 //// Second position, k2 = f(t + h/2, y + k1*h/2)
 208
 209                 status2 = status.clone();
 210                 status2.setSimulationTime(status.getSimulationTime() + store.timestep / 2);
 211                 status2.setRocketPosition(status.getRocketPosition().add(k1.v.multiply(store.timestep / 2)));
 212                 status2.setRocketVelocity(status.getRocketVelocity().add(k1.a.multiply(store.timestep / 2)));
 213                 status2.setRocketOrientationQuaternion(status.getRocketOrientationQuaternion().multiplyLeft(Quaternion.rotation(k1.rv.multiply(store.timestep / 2))));
 214                 status2.setRocketRotationVelocity(status.getRocketRotationVelocity().add(k1.ra.multiply(store.timestep / 2)));
 215
 216                 k2 = computeParameters(status2, store);
 217
 218
 219                 //// Third position, k3 = f(t + h/2, y + k2*h/2)
 220
 221                 status2 = status.clone();
 222                 status2.setSimulationTime(status.getSimulationTime() + store.timestep / 2);
 223                 status2.setRocketPosition(status.getRocketPosition().add(k2.v.multiply(store.timestep / 2)));
 224                 status2.setRocketVelocity(status.getRocketVelocity().add(k2.a.multiply(store.timestep / 2)));
 225                 status2.setRocketOrientationQuaternion(status2.getRocketOrientationQuaternion().multiplyLeft(Quaternion.rotation(k2.rv.multiply(store.timestep / 2))));
 226                 status2.setRocketRotationVelocity(status.getRocketRotationVelocity().add(k2.ra.multiply(store.timestep / 2)));
 227
 228                 k3 = computeParameters(status2, store);
 229
 230
 231                 //// Fourth position, k4 = f(t + h, y + k3*h)
 232
 233                 status2 = status.clone();
 234                 status2.setSimulationTime(status.getSimulationTime() + store.timestep);
 235                 status2.setRocketPosition(status.getRocketPosition().add(k3.v.multiply(store.timestep)));
 236                 status2.setRocketVelocity(status.getRocketVelocity().add(k3.a.multiply(store.timestep)));
 237                 status2.setRocketOrientationQuaternion(status2.getRocketOrientationQuaternion().multiplyLeft(Quaternion.rotation(k3.rv.multiply(store.timestep))));
 238                 status2.setRocketRotationVelocity(status.getRocketRotationVelocity().add(k3.ra.multiply(store.timestep)));
 239
 240                 k4 = computeParameters(status2, store);
 241
 242
 243                 //// Sum all together,  y(n+1) = y(n) + h*(k1 + 2*k2 + 2*k3 + k4)/6
 244
 245
 246
 247                 Coordinate deltaV, deltaP, deltaR, deltaO;
 248                 deltaV = k2.a.add(k3.a).multiply(2).add(k1.a).add(k4.a).multiply(store.timestep / 6);
 249                 deltaP = k2.v.add(k3.v).multiply(2).add(k1.v).add(k4.v).multiply(store.timestep / 6);
 250                 deltaR = k2.ra.add(k3.ra).multiply(2).add(k1.ra).add(k4.ra).multiply(store.timestep / 6);
 251                 deltaO = k2.rv.add(k3.rv).multiply(2).add(k1.rv).add(k4.rv).multiply(store.timestep / 6);
 252
 253
 254
 255                 status.setRocketVelocity(status.getRocketVelocity().add(deltaV));
 256                 status.setRocketPosition(status.getRocketPosition().add(deltaP));
 257                 status.setRocketRotationVelocity(status.getRocketRotationVelocity().add(deltaR));
 258                 status.setRocketOrientationQuaternion(status.getRocketOrientationQuaternion().multiplyLeft(Quaternion.rotation(deltaO)).normalizeIfNecessary());
 259
 260                 WorldCoordinate w = status.getSimulationConditions().getLaunchSite();
 261                 w = status.getSimulationConditions().getGeodeticComputation().addCoordinate(w, status.getRocketPosition());
 262                 status.setRocketWorldPosition(w);
 263
 264                 status.setSimulationTime(status.getSimulationTime() + store.timestep);
 265
 266                 status.setPreviousTimeStep(store.timestep);
 267
 268                 // Verify that values don't run out of range
 269                 if (status.getRocketVelocity().length2() > 1e18 ||
 270                                 status.getRocketPosition().length2() > 1e18 ||
 271                                 status.getRocketRotationVelocity().length2() > 1e18) {
 272                         throw new SimulationCalculationException(trans.get("error.valuesTooLarge"));
 273                 }
 274         }
 275
 276
 277
 278
 279
 280         private RK4Parameters computeParameters(RK4SimulationStatus status, DataStore dataStore)
 281                         throws SimulationException {
 282                 RK4Parameters params = new RK4Parameters();
 283
 284                 //              if (dataStore == null) {
 285                 //                      dataStore = new DataStore();
 286                 //              }
 287
 288                 calculateAcceleration(status, dataStore);
 289                 params.a = dataStore.linearAcceleration;
 290                 params.ra = dataStore.angularAcceleration;
 291                 params.v = status.getRocketVelocity();
 292                 params.rv = status.getRocketRotationVelocity();
 293
 294                 checkNaN(params.a);
 295                 checkNaN(params.ra);
 296                 checkNaN(params.v);
 297                 checkNaN(params.rv);
 298
 299                 return params;
 300         }
 301
 302
 303
 304
 305
 306         /**
 307          * Calculate the linear and angular acceleration at the given status.  The results
 308          * are stored in the fields {@link #linearAcceleration} and {@link #angularAcceleration}.
 309          *
 310          * @param status   the status of the rocket.
 311          * @throws SimulationException
 312          */
 313         private void calculateAcceleration(RK4SimulationStatus status, DataStore store) throws SimulationException {
 314
 315                 // Call pre-listeners
 316                 store.accelerationData = SimulationListenerHelper.firePreAccelerationCalculation(status);
 317                 if (store.accelerationData != null) {
 318                         return;
 319                 }
 320
 321                 // Compute the forces affecting the rocket
 322                 calculateForces(status, store);
 323
 324                 // Calculate mass data
 325                 store.massData = calculateMassData(status);
 326
 327
 328                 // Calculate the forces from the aerodynamic coefficients
 329
 330                 double dynP = (0.5 * store.flightConditions.getAtmosphericConditions().getDensity() *
 331                                         MathUtil.pow2(store.flightConditions.getVelocity()));
 332                 double refArea = store.flightConditions.getRefArea();
 333                 double refLength = store.flightConditions.getRefLength();
 334
 335
 336                 // Linear forces in rocket coordinates
 337                 store.dragForce = store.forces.getCaxial() * dynP * refArea;
 338                 double fN = store.forces.getCN() * dynP * refArea;
 339                 double fSide = store.forces.getCside() * dynP * refArea;
 340
 341                 double forceZ = store.thrustForce - store.dragForce;
 342
 343                 store.linearAcceleration = new Coordinate(-fN / store.massData.getCG().weight,
 344                                         -fSide / store.massData.getCG().weight,
 345                                         forceZ / store.massData.getCG().weight);
 346
 347                 store.linearAcceleration = store.thetaRotation.rotateZ(store.linearAcceleration);
 348
 349                 // Convert into rocket world coordinates
 350                 store.linearAcceleration = status.getRocketOrientationQuaternion().rotate(store.linearAcceleration);
 351
 352                 // add effect of gravity
 353                 store.gravity = modelGravity(status);
 354                 store.linearAcceleration = store.linearAcceleration.sub(0, 0, store.gravity);
 355
 356                 // add effect of Coriolis acceleration
 357                 store.coriolisAcceleration = status.getSimulationConditions().getGeodeticComputation()
 358                                 .getCoriolisAcceleration(status.getRocketWorldPosition(), status.getRocketVelocity());
 359                 store.linearAcceleration = store.linearAcceleration.add(store.coriolisAcceleration);
 360
 361                 // If still on the launch rod, project acceleration onto launch rod direction and
 362                 // set angular acceleration to zero.
 363                 if (!status.isLaunchRodCleared()) {
 364
 365                         store.linearAcceleration = status.getLaunchRodDirection().multiply(
 366                                                 store.linearAcceleration.dot(status.getLaunchRodDirection()));
 367                         store.angularAcceleration = Coordinate.NUL;
 368                         store.rollAcceleration = 0;
 369                         store.lateralPitchAcceleration = 0;
 370
 371                 } else {
 372
 373                         // Shift moments to CG
 374                         double Cm = store.forces.getCm() - store.forces.getCN() * store.massData.getCG().x / refLength;
 375                         double Cyaw = store.forces.getCyaw() - store.forces.getCside() * store.massData.getCG().x / refLength;
 376
 377                         // Compute moments
 378                         double momX = -Cyaw * dynP * refArea * refLength;
 379                         double momY = Cm * dynP * refArea * refLength;
 380                         double momZ = store.forces.getCroll() * dynP * refArea * refLength;
 381
 382                         // Compute acceleration in rocket coordinates
 383                         store.angularAcceleration = new Coordinate(momX / store.massData.getLongitudinalInertia(),
 384                                                 momY / store.massData.getLongitudinalInertia(),
 385                                                 momZ / store.massData.getRotationalInertia());
 386
 387                         store.rollAcceleration = store.angularAcceleration.z;
 388                         // TODO: LOW: This should be hypot, but does it matter?
 389                         store.lateralPitchAcceleration = MathUtil.max(Math.abs(store.angularAcceleration.x),
 390                                                 Math.abs(store.angularAcceleration.y));
 391
 392                         store.angularAcceleration = store.thetaRotation.rotateZ(store.angularAcceleration);
 393
 394                         // Convert to world coordinates
 395                         store.angularAcceleration = status.getRocketOrientationQuaternion().rotate(store.angularAcceleration);
 396
 397                 }
 398
 399                 // Call post-listeners
 400                 store.accelerationData = SimulationListenerHelper.firePostAccelerationCalculation(status, store.accelerationData);
 401         }
 402
 403
 404         /**
 405          * Calculate the aerodynamic forces into the data store.  This method also handles
 406          * whether to include aerodynamic computation warnings or not.
 407          */
 408         private void calculateForces(RK4SimulationStatus status, DataStore store) throws SimulationException {
 409
 410                 // Call pre-listeners
 411                 store.forces = SimulationListenerHelper.firePreAerodynamicCalculation(status);
 412                 if (store.forces != null) {
 413                         return;
 414                 }
 415
 416                 // Compute flight conditions
 417                 calculateFlightConditions(status, store);
 418
 419                 /*
 420                  * Check whether to store warnings or not.  Warnings are ignored when on the
 421                  * launch rod or 0.25 seconds after departure, and when the velocity has dropped
 422                  * below 20% of the max. velocity.
 423                  */
 424                 WarningSet warnings = status.getWarnings();
 425                 status.setMaxZVelocity(MathUtil.max(status.getMaxZVelocity(), status.getRocketVelocity().z));
 426
 427                 if (!status.isLaunchRodCleared()) {
 428                         warnings = null;
 429                 } else {
 430                         if (status.getRocketVelocity().z < 0.2 * status.getMaxZVelocity())
 431                                 warnings = null;
 432                         if (status.getStartWarningTime() < 0)
 433                                 status.setStartWarningTime(status.getSimulationTime() + 0.25);
 434                 }
 435                 if (status.getSimulationTime() < status.getStartWarningTime())
 436                         warnings = null;
 437
 438
 439                 // Calculate aerodynamic forces
 440                 store.forces = status.getSimulationConditions().getAerodynamicCalculator()
 441                                 .getAerodynamicForces(status.getConfiguration(), store.flightConditions, warnings);
 442
 443
 444                 // Add very small randomization to yaw & pitch moments to prevent over-perfect flight
 445                 // TODO: HIGH: This should rather be performed as a listener
 446                 store.forces.setCm(store.forces.getCm() + (PITCH_YAW_RANDOM * 2 * (random.nextDouble() - 0.5)));
 447                 store.forces.setCyaw(store.forces.getCyaw() + (PITCH_YAW_RANDOM * 2 * (random.nextDouble() - 0.5)));
 448
 449
 450                 // Call post-listeners
 451                 store.forces = SimulationListenerHelper.firePostAerodynamicCalculation(status, store.forces);
 452         }
 453
 454
 455
 456         /**
 457          * Calculate and return the flight conditions for the current rocket status.
 458          * Listeners can override these if necessary.
 459          * <p>
 460          * Additionally the fields thetaRotation and lateralPitchRate are defined in
 461          * the data store, and can be used after calling this method.
 462          */
 463         private void calculateFlightConditions(RK4SimulationStatus status, DataStore store)
 464                         throws SimulationException {
 465
 466                 // Call pre listeners, allow complete override
 467                 store.flightConditions = SimulationListenerHelper.firePreFlightConditions(
 468                                 status);
 469                 if (store.flightConditions != null) {
 470                         // Compute the store values
 471                         store.thetaRotation = new Rotation2D(store.flightConditions.getTheta());
 472                         store.lateralPitchRate = Math.hypot(store.flightConditions.getPitchRate(), store.flightConditions.getYawRate());
 473                         return;
 474                 }
 475
 476
 477
 478                 //// Atmospheric conditions
 479                 AtmosphericConditions atmosphere = modelAtmosphericConditions(status);
 480                 store.flightConditions = new FlightConditions(status.getConfiguration());
 481                 store.flightConditions.setAtmosphericConditions(atmosphere);
 482
 483
 484                 //// Local wind speed and direction
 485                 Coordinate windSpeed = modelWindVelocity(status);
 486                 Coordinate airSpeed = status.getRocketVelocity().add(windSpeed);
 487                 airSpeed = status.getRocketOrientationQuaternion().invRotate(airSpeed);
 488
 489
 490                 // Lateral direction:
 491                 double len = MathUtil.hypot(airSpeed.x, airSpeed.y);
 492                 if (len > 0.0001) {
 493                         store.thetaRotation = new Rotation2D(airSpeed.y / len, airSpeed.x / len);
 494                         store.flightConditions.setTheta(Math.atan2(airSpeed.y, airSpeed.x));
 495                 } else {
 496                         store.thetaRotation = Rotation2D.ID;
 497                         store.flightConditions.setTheta(0);
 498                 }
 499
 500                 double velocity = airSpeed.length();
 501                 store.flightConditions.setVelocity(velocity);
 502                 if (velocity > 0.01) {
 503                         // aoa must be calculated from the monotonous cosine
 504                         // sine can be calculated by a simple division
 505                         store.flightConditions.setAOA(Math.acos(airSpeed.z / velocity), len / velocity);
 506                 } else {
 507                         store.flightConditions.setAOA(0);
 508                 }
 509
 510
 511                 // Roll, pitch and yaw rate
 512                 Coordinate rot = status.getRocketOrientationQuaternion().invRotate(status.getRocketRotationVelocity());
 513                 rot = store.thetaRotation.invRotateZ(rot);
 514
 515                 store.flightConditions.setRollRate(rot.z);
 516                 if (len < 0.001) {
 517                         store.flightConditions.setPitchRate(0);
 518                         store.flightConditions.setYawRate(0);
 519                         store.lateralPitchRate = 0;
 520                 } else {
 521                         store.flightConditions.setPitchRate(rot.y);
 522                         store.flightConditions.setYawRate(rot.x);
 523                         // TODO: LOW: set this as power of two?
 524                         store.lateralPitchRate = MathUtil.hypot(rot.x, rot.y);
 525                 }
 526
 527
 528                 // Call post listeners
 529                 FlightConditions c = SimulationListenerHelper.firePostFlightConditions(
 530                                 status, store.flightConditions);
 531                 if (c != store.flightConditions) {
 532                         // Listeners changed the values, recalculate data store
 533                         store.flightConditions = c;
 534                         store.thetaRotation = new Rotation2D(store.flightConditions.getTheta());
 535                         store.lateralPitchRate = Math.hypot(store.flightConditions.getPitchRate(), store.flightConditions.getYawRate());
 536                 }
 537
 538         }
 539
 540
 541
 542         private void storeData(RK4SimulationStatus status, DataStore store) {
 543
 544                 FlightDataBranch data = status.getFlightData();
 545                 boolean extra = status.getSimulationConditions().isCalculateExtras();
 546
 547                 data.addPoint();
 548                 data.setValue(FlightDataType.TYPE_TIME, status.getSimulationTime());
 549                 data.setValue(FlightDataType.TYPE_ALTITUDE, status.getRocketPosition().z);
 550                 data.setValue(FlightDataType.TYPE_POSITION_X, status.getRocketPosition().x);
 551                 data.setValue(FlightDataType.TYPE_POSITION_Y, status.getRocketPosition().y);
 552
 553                 data.setValue(FlightDataType.TYPE_LATITUDE, status.getRocketWorldPosition().getLatitudeRad());
 554                 data.setValue(FlightDataType.TYPE_LONGITUDE, status.getRocketWorldPosition().getLongitudeRad());
 555                 if (status.getSimulationConditions().getGeodeticComputation() != GeodeticComputationStrategy.FLAT) {
 556                         data.setValue(FlightDataType.TYPE_CORIOLIS_ACCELERATION, store.coriolisAcceleration.length());
 557                 }
 558
 559                 if (extra) {
 560                         data.setValue(FlightDataType.TYPE_POSITION_XY,
 561                                         MathUtil.hypot(status.getRocketPosition().x, status.getRocketPosition().y));
 562                         data.setValue(FlightDataType.TYPE_POSITION_DIRECTION,
 563                                         Math.atan2(status.getRocketPosition().y, status.getRocketPosition().x));
 564
 565                         data.setValue(FlightDataType.TYPE_VELOCITY_XY,
 566                                         MathUtil.hypot(status.getRocketVelocity().x, status.getRocketVelocity().y));
 567
 568                         if (store.linearAcceleration != null) {
 569                                 data.setValue(FlightDataType.TYPE_ACCELERATION_XY,
 570                                                 MathUtil.hypot(store.linearAcceleration.x, store.linearAcceleration.y));
 571
 572                                 data.setValue(FlightDataType.TYPE_ACCELERATION_TOTAL, store.linearAcceleration.length());
 573                         }
 574
 575                         if (store.flightConditions != null) {
 576                                 double Re = (store.flightConditions.getVelocity() *
 577                                                 status.getConfiguration().getLength() /
 578                                                 store.flightConditions.getAtmosphericConditions().getKinematicViscosity());
 579                                 data.setValue(FlightDataType.TYPE_REYNOLDS_NUMBER, Re);
 580                         }
 581                 }
 582
 583                 data.setValue(FlightDataType.TYPE_VELOCITY_Z, status.getRocketVelocity().z);
 584                 if (store.linearAcceleration != null) {
 585                         data.setValue(FlightDataType.TYPE_ACCELERATION_Z, store.linearAcceleration.z);
 586                 }
 587
 588                 if (store.flightConditions != null) {
 589                         data.setValue(FlightDataType.TYPE_VELOCITY_TOTAL, status.getRocketVelocity().length());
 590                         data.setValue(FlightDataType.TYPE_MACH_NUMBER, store.flightConditions.getMach());
 591                 }
 592
 593                 if (store.massData != null) {
 594                         data.setValue(FlightDataType.TYPE_CG_LOCATION, store.massData.getCG().x);
 595                 }
 596                 if (status.isLaunchRodCleared()) {
 597                         // Don't include CP and stability with huge launch AOA
 598                         if (store.forces != null) {
 599                                 data.setValue(FlightDataType.TYPE_CP_LOCATION, store.forces.getCP().x);
 600                         }
 601                         if (store.forces != null && store.flightConditions != null && store.massData != null) {
 602                                 data.setValue(FlightDataType.TYPE_STABILITY,
 603                                                 (store.forces.getCP().x - store.massData.getCG().x) / store.flightConditions.getRefLength());
 604                         }
 605                 }
 606                 if (store.massData != null) {
 607                         data.setValue(FlightDataType.TYPE_MASS, store.massData.getCG().weight);
 608                         data.setValue(FlightDataType.TYPE_PROPELLANT_MASS, store.massData.getPropellantMass());
 609                         data.setValue(FlightDataType.TYPE_LONGITUDINAL_INERTIA, store.massData.getLongitudinalInertia());
 610                         data.setValue(FlightDataType.TYPE_ROTATIONAL_INERTIA, store.massData.getRotationalInertia());
 611                 }
 612
 613                 data.setValue(FlightDataType.TYPE_THRUST_FORCE, store.thrustForce);
 614                 data.setValue(FlightDataType.TYPE_DRAG_FORCE, store.dragForce);
 615                 data.setValue(FlightDataType.TYPE_GRAVITY, store.gravity);
 616
 617                 if (status.isLaunchRodCleared() && store.forces != null) {
 618                         if (store.massData != null && store.flightConditions != null) {
 619                                 data.setValue(FlightDataType.TYPE_PITCH_MOMENT_COEFF,
 620                                                 store.forces.getCm() - store.forces.getCN() * store.massData.getCG().x / store.flightConditions.getRefLength());
 621                                 data.setValue(FlightDataType.TYPE_YAW_MOMENT_COEFF,
 622                                                 store.forces.getCyaw() - store.forces.getCside() * store.massData.getCG().x / store.flightConditions.getRefLength());
 623                         }
 624                         data.setValue(FlightDataType.TYPE_NORMAL_FORCE_COEFF, store.forces.getCN());
 625                         data.setValue(FlightDataType.TYPE_SIDE_FORCE_COEFF, store.forces.getCside());
 626                         data.setValue(FlightDataType.TYPE_ROLL_MOMENT_COEFF, store.forces.getCroll());
 627                         data.setValue(FlightDataType.TYPE_ROLL_FORCING_COEFF, store.forces.getCrollForce());
 628                         data.setValue(FlightDataType.TYPE_ROLL_DAMPING_COEFF, store.forces.getCrollDamp());
 629                         data.setValue(FlightDataType.TYPE_PITCH_DAMPING_MOMENT_COEFF,
 630                                         store.forces.getPitchDampingMoment());
 631                 }
 632
 633                 if (store.forces != null) {
 634                         data.setValue(FlightDataType.TYPE_DRAG_COEFF, store.forces.getCD());
 635                         data.setValue(FlightDataType.TYPE_AXIAL_DRAG_COEFF, store.forces.getCaxial());
 636                         data.setValue(FlightDataType.TYPE_FRICTION_DRAG_COEFF, store.forces.getFrictionCD());
 637                         data.setValue(FlightDataType.TYPE_PRESSURE_DRAG_COEFF, store.forces.getPressureCD());
 638                         data.setValue(FlightDataType.TYPE_BASE_DRAG_COEFF, store.forces.getBaseCD());
 639                 }
 640
 641                 if (store.flightConditions != null) {
 642                         data.setValue(FlightDataType.TYPE_REFERENCE_LENGTH, store.flightConditions.getRefLength());
 643                         data.setValue(FlightDataType.TYPE_REFERENCE_AREA, store.flightConditions.getRefArea());
 644
 645                         data.setValue(FlightDataType.TYPE_PITCH_RATE, store.flightConditions.getPitchRate());
 646                         data.setValue(FlightDataType.TYPE_YAW_RATE, store.flightConditions.getYawRate());
 647                         data.setValue(FlightDataType.TYPE_ROLL_RATE, store.flightConditions.getRollRate());
 648
 649                         data.setValue(FlightDataType.TYPE_AOA, store.flightConditions.getAOA());
 650                 }
 651
 652
 653                 if (extra) {
 654                         Coordinate c = status.getRocketOrientationQuaternion().rotateZ();
 655                         double theta = Math.atan2(c.z, MathUtil.hypot(c.x, c.y));
 656                         double phi = Math.atan2(c.y, c.x);
 657                         if (phi < -(Math.PI - 0.0001))
 658                                 phi = Math.PI;
 659                         data.setValue(FlightDataType.TYPE_ORIENTATION_THETA, theta);
 660                         data.setValue(FlightDataType.TYPE_ORIENTATION_PHI, phi);
 661                 }
 662
 663                 data.setValue(FlightDataType.TYPE_WIND_VELOCITY, store.windSpeed);
 664
 665                 if (store.flightConditions != null) {
 666                         data.setValue(FlightDataType.TYPE_AIR_TEMPERATURE,
 667                                         store.flightConditions.getAtmosphericConditions().getTemperature());
 668                         data.setValue(FlightDataType.TYPE_AIR_PRESSURE,
 669                                         store.flightConditions.getAtmosphericConditions().getPressure());
 670                         data.setValue(FlightDataType.TYPE_SPEED_OF_SOUND,
 671                                         store.flightConditions.getAtmosphericConditions().getMachSpeed());
 672                 }
 673
 674
 675                 data.setValue(FlightDataType.TYPE_TIME_STEP, store.timestep);
 676                 data.setValue(FlightDataType.TYPE_COMPUTATION_TIME,
 677                                 (System.nanoTime() - status.getSimulationStartWallTime()) / 1000000000.0);
 678         }
 679
 680
 681
 682
 683         private static class RK4Parameters {
 684                 /** Linear acceleration */
 685                 public Coordinate a;
 686                 /** Linear velocity */
 687                 public Coordinate v;
 688                 /** Rotational acceleration */
 689                 public Coordinate ra;
 690                 /** Rotational velocity */
 691                 public Coordinate rv;
 692         }
 693
 694         private static class DataStore {
 695                 public double timestep = Double.NaN;
 696
 697                 public AccelerationData accelerationData;
 698
 699                 public AtmosphericConditions atmosphericConditions;
 700
 701                 public FlightConditions flightConditions;
 702
 703                 public double longitudinalAcceleration = Double.NaN;
 704
 705                 public MassData massData;
 706
 707                 public Coordinate coriolisAcceleration;
 708
 709                 public Coordinate linearAcceleration;
 710                 public Coordinate angularAcceleration;
 711
 712                 // set by calculateFlightConditions and calculateAcceleration:
 713                 public AerodynamicForces forces;
 714                 public double windSpeed = Double.NaN;
 715                 public double gravity = Double.NaN;
 716                 public double thrustForce = Double.NaN;
 717                 public double dragForce = Double.NaN;
 718                 public double lateralPitchRate = Double.NaN;
 719
 720                 public double rollAcceleration = Double.NaN;
 721                 public double lateralPitchAcceleration = Double.NaN;
 722
 723                 public Rotation2D thetaRotation;
 724
 725         }
 726
 727 }