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

   1 package net.sf.openrocket.simulation;
   2
   3 import java.util.Collection;
   4 import java.util.Random;
   5
   6 import net.sf.openrocket.aerodynamics.AerodynamicCalculator;
   7 import net.sf.openrocket.aerodynamics.AerodynamicForces;
   8 import net.sf.openrocket.aerodynamics.AtmosphericConditions;
   9 import net.sf.openrocket.aerodynamics.FlightConditions;
  10 import net.sf.openrocket.aerodynamics.GravityModel;
  11 import net.sf.openrocket.aerodynamics.Warning;
  12 import net.sf.openrocket.aerodynamics.WarningSet;
  13 import net.sf.openrocket.aerodynamics.WindSimulator;
  14 import net.sf.openrocket.rocketcomponent.Configuration;
  15 import net.sf.openrocket.rocketcomponent.LaunchLug;
  16 import net.sf.openrocket.rocketcomponent.RocketComponent;
  17 import net.sf.openrocket.simulation.exception.SimulationException;
  18 import net.sf.openrocket.util.Coordinate;
  19 import net.sf.openrocket.util.MathUtil;
  20 import net.sf.openrocket.util.Quaternion;
  21 import net.sf.openrocket.util.Rotation2D;
  22
  23
  24 public class RK4Simulator extends FlightSimulator {
  25
  26         private static final boolean DEBUG = false;
  27
  28         /**
  29          * A recommended reasonably accurate time step.
  30          */
  31         public static final double RECOMMENDED_TIME_STEP = 0.05;
  32
  33         /**
  34          * A recommended maximum angle step value.
  35          */
  36         public static final double RECOMMENDED_ANGLE_STEP = 3*Math.PI/180;
  37
  38         /**
  39          * A random amount that is added to pitch and yaw coefficients, plus or minus.
  40          */
  41         public static final double PITCH_YAW_RANDOM = 0.0005;
  42
  43         /**
  44          * Maximum roll step allowed.  This is selected as an uneven division of the full
  45          * circle so that the simulation will sample the most wind directions
  46          */
  47         private static final double MAX_ROLL_STEP_ANGLE = 2*28.32 * Math.PI/180;
  48 //      private static final double MAX_ROLL_STEP_ANGLE = 8.32 * Math.PI/180;
  49
  50         private static final double MAX_ROLL_RATE_CHANGE = 2 * Math.PI/180;
  51         private static final double MAX_PITCH_CHANGE = 4 * Math.PI/180;
  52
  53
  54
  55         /* Single instance so it doesn't have to be created each semi-step. */
  56         private final FlightConditions flightConditions = new FlightConditions(null);
  57
  58
  59         private final Random random = new Random();
  60
  61
  62         private Coordinate linearAcceleration;
  63         private Coordinate angularAcceleration;
  64
  65         // set by calculateFlightConditions and calculateAcceleration:
  66         private double timestep;
  67         private double oldTimestep;
  68         private AerodynamicForces forces;
  69         private double windSpeed;
  70         private double thrustForce, dragForce;
  71         private double lateralPitchRate = 0;
  72
  73         private double rollAcceleration = 0;
  74         private double lateralPitchAcceleration = 0;
  75
  76         private double maxVelocityZ = 0;
  77         private double startWarningTime = -1;
  78
  79         private Rotation2D thetaRotation;
  80
  81
  82         public RK4Simulator() {
  83                 super();
  84         }
  85
  86
  87         public RK4Simulator(AerodynamicCalculator calculator) {
  88                 super(calculator);
  89         }
  90
  91
  92
  93
  94
  95         @Override
  96         protected RK4SimulationStatus initializeSimulation(Configuration configuration,
  97                         SimulationConditions simulation) {
  98
  99                 RK4SimulationStatus status = new RK4SimulationStatus();
 100
 101                 status.startConditions = simulation;
 102
 103                 status.configuration = configuration;
 104                 // TODO: LOW: Branch names
 105                 status.flightData = new FlightDataBranch("Main", FlightDataBranch.TYPE_TIME);
 106                 status.launchRod = true;
 107                 status.time = 0.0;
 108                 status.simulationStartTime = System.nanoTime();
 109
 110                 status.launchRodDirection = new Coordinate(
 111                                 Math.sin(simulation.getLaunchRodAngle()) *
 112                                 Math.cos(simulation.getLaunchRodDirection()),
 113                                 Math.sin(simulation.getLaunchRodAngle()) *
 114                                 Math.sin(simulation.getLaunchRodDirection()),
 115                                 Math.cos(simulation.getLaunchRodAngle())
 116                 );
 117                 status.launchRodLength = simulation.getLaunchRodLength();
 118
 119                 // Take into account launch lug positions
 120                 double lugPosition = Double.NaN;
 121                 for (RocketComponent c: configuration) {
 122                         if (c instanceof LaunchLug) {
 123                                 double pos = c.toAbsolute(new Coordinate(c.getLength()))[0].x;
 124                                 if (Double.isNaN(lugPosition) || pos > lugPosition) {
 125                                         lugPosition = pos;
 126                                 }
 127                         }
 128                 }
 129                 if (!Double.isNaN(lugPosition)) {
 130                         double maxX = 0;
 131                         for (Coordinate c: configuration.getBounds()) {
 132                                 if (c.x > maxX)
 133                                         maxX = c.x;
 134                         }
 135                         if (maxX >= lugPosition) {
 136                                 status.launchRodLength = Math.max(0,
 137                                                 status.launchRodLength - (maxX - lugPosition));
 138                         }
 139                 }
 140
 141
 142                 Quaternion o = new Quaternion();
 143
 144                 // Initialize to roll angle with least stability w.r.t. the wind
 145                 FlightConditions cond = new FlightConditions(configuration);
 146                 calculator.getWorstCP(cond, null);
 147                 double angle = -cond.getTheta() - simulation.getLaunchRodDirection();
 148                 o.multiplyLeft(Quaternion.rotation(new Coordinate(0, 0, angle)));
 149
 150                 // Launch rod angle and direction
 151                 o.multiplyLeft(Quaternion.rotation(
 152                                 new Coordinate(0, simulation.getLaunchRodAngle(), 0)));
 153                 o.multiplyLeft(Quaternion.rotation(
 154                                 new Coordinate(0, 0, simulation.getLaunchRodDirection())));
 155
 156                 status.orientation = o;
 157                 status.position = Coordinate.NUL;
 158                 status.velocity = Coordinate.NUL;
 159                 status.rotation = Coordinate.NUL;
 160
 161                 /*
 162                  * Force a very small deviation to the wind speed to avoid insanely
 163                  * perfect conditions (rocket dropping at exactly 180 deg AOA).
 164                  */
 165                 status.windSimulator = new WindSimulator();
 166                 status.windSimulator.setAverage(simulation.getWindSpeedAverage());
 167                 status.windSimulator.setStandardDeviation(
 168                                 Math.max(simulation.getWindSpeedDeviation(), 0.005));
 169 //              status.windSimulator.reset();
 170
 171                 status.gravityModel = new GravityModel(simulation.getLaunchLatitude());
 172
 173                 rollAcceleration = 0;
 174                 lateralPitchAcceleration = 0;
 175                 oldTimestep = -1;
 176                 maxVelocityZ = 0;
 177                 startWarningTime = -1;
 178
 179                 return status;
 180         }
 181
 182
 183
 184         @Override
 185         protected Collection<FlightEvent> step(SimulationConditions simulation,
 186                         SimulationStatus simulationStatus) throws SimulationException {
 187
 188                 RK4SimulationStatus status = (RK4SimulationStatus)simulationStatus;
 189
 190                 ////////  Perform RK4 integration:  ////////
 191
 192                 Coordinate k1a, k1v, k1ra, k1rv; // Acceleration, velocity, rot.acc, rot.vel
 193                 Coordinate k2a, k2v, k2ra, k2rv;
 194                 Coordinate k3a, k3v, k3ra, k3rv;
 195                 Coordinate k4a, k4v, k4ra, k4rv;
 196                 RK4SimulationStatus status2;
 197
 198
 199                 // Calculate time step and store data after first call to calculateFlightConditions
 200                 calculateFlightConditions(status);
 201
 202
 203                 /*
 204                  * Select the time step to use.  It is the minimum of the following:
 205                  *  1. the user-specified time step
 206                  *  2. the maximum pitch step angle limit
 207                  *  3. the maximum roll step angle limit
 208                  *  4. the maximum roll rate change limit (using previous step acceleration)
 209                  *  5. the maximum pitch change limit (using previous step acceleration)
 210                  *
 211                  * The last two are required since near the steady-state roll rate the roll rate
 212                  * may oscillate significantly even between the sub-steps of the RK4 integration.
 213                  *
 214                  * Additionally if the time step is longer than the previous time step, the
 215                  * increase is limited to 50% of the previous time step.
 216                  */
 217                 double dt1 = simulation.getTimeStep();
 218                 double dt2 = simulation.getMaximumStepAngle() / lateralPitchRate;
 219                 double dt3 = Math.abs(MAX_ROLL_STEP_ANGLE / flightConditions.getRollRate());
 220                 double dt4 = Math.abs(MAX_ROLL_RATE_CHANGE / rollAcceleration);
 221                 double dt5 = Math.abs(MAX_PITCH_CHANGE / lateralPitchAcceleration);
 222                 timestep = MathUtil.min(dt1,dt2,dt3);
 223                 timestep = MathUtil.min(timestep,dt4,dt5);
 224
 225                 if (status.launchRod) {
 226                         // Use 1/5th time step while on launch rod
 227                         timestep = MathUtil.min(timestep, simulation.getTimeStep()/5);
 228                 }
 229
 230                 if (oldTimestep > 0.0001 && oldTimestep < timestep) {
 231                         timestep = Math.min(timestep, oldTimestep*1.5);
 232                 }
 233
 234 //              if (timestep < 0.001)
 235 //                      timestep = 0.001;
 236                 if (timestep < simulation.getTimeStep() / 10)
 237                         timestep = simulation.getTimeStep() / 10;
 238
 239                 oldTimestep = timestep;
 240                 if (DEBUG) {
 241                         System.out.printf("Time step (t=%.3f): %.3f  dt1=%.3f dt2=%.3f dt3=%.3f dt4=%.3f dt5=%.3f\n",
 242                                         status.time, timestep,dt1,dt2,dt3,dt4,dt5);
 243                 }
 244
 245
 246                 //// First position, k1 = f(t, y)
 247
 248                 //calculateFlightConditions already called
 249                 calculateAcceleration(status);
 250                 k1a = linearAcceleration;
 251                 k1ra = angularAcceleration;
 252                 k1v = status.velocity;
 253                 k1rv = status.rotation;
 254
 255
 256                 // Store the flight information
 257                 storeData(status);
 258
 259
 260
 261                 //// Second position, k2 = f(t + h/2, y + k1*h/2)
 262
 263                 status2 = status.clone();
 264                 status2.time = status.time + timestep/2;
 265                 status2.position = status.position.add(k1v.multiply(timestep/2));
 266                 status2.velocity = status.velocity.add(k1a.multiply(timestep/2));
 267                 status2.orientation.multiplyLeft(Quaternion.rotation(k1rv.multiply(timestep/2)));
 268                 status2.rotation = status.rotation.add(k1ra.multiply(timestep/2));
 269
 270                 calculateFlightConditions(status2);
 271                 calculateAcceleration(status2);
 272                 k2a = linearAcceleration;
 273                 k2ra = angularAcceleration;
 274                 k2v = status2.velocity;
 275                 k2rv = status2.rotation;
 276
 277
 278                 //// Third position, k3 = f(t + h/2, y + k2*h/2)
 279
 280                 status2.orientation = status.orientation.clone();  // All others are set explicitly
 281                 status2.position = status.position.add(k2v.multiply(timestep/2));
 282                 status2.velocity = status.velocity.add(k2a.multiply(timestep/2));
 283                 status2.orientation.multiplyLeft(Quaternion.rotation(k2rv.multiply(timestep/2)));
 284                 status2.rotation = status.rotation.add(k2ra.multiply(timestep/2));
 285
 286                 calculateFlightConditions(status2);
 287                 calculateAcceleration(status2);
 288                 k3a = linearAcceleration;
 289                 k3ra = angularAcceleration;
 290                 k3v = status2.velocity;
 291                 k3rv = status2.rotation;
 292
 293
 294
 295                 //// Fourth position, k4 = f(t + h, y + k3*h)
 296
 297                 status2.orientation = status.orientation.clone();  // All others are set explicitly
 298                 status2.time = status.time + timestep;
 299                 status2.position = status.position.add(k3v.multiply(timestep));
 300                 status2.velocity = status.velocity.add(k3a.multiply(timestep));
 301                 status2.orientation.multiplyLeft(Quaternion.rotation(k3rv.multiply(timestep)));
 302                 status2.rotation = status.rotation.add(k3ra.multiply(timestep));
 303
 304                 calculateFlightConditions(status2);
 305                 calculateAcceleration(status2);
 306                 k4a = linearAcceleration;
 307                 k4ra = angularAcceleration;
 308                 k4v = status2.velocity;
 309                 k4rv = status2.rotation;
 310
 311
 312
 313                 //// Sum all together,  y(n+1) = y(n) + h*(k1 + 2*k2 + 2*k3 + k4)/6
 314
 315                 Coordinate deltaV, deltaP, deltaR, deltaO;
 316                 deltaV = k2a.add(k3a).multiply(2).add(k1a).add(k4a).multiply(timestep/6);
 317                 deltaP = k2v.add(k3v).multiply(2).add(k1v).add(k4v).multiply(timestep/6);
 318                 deltaR = k2ra.add(k3ra).multiply(2).add(k1ra).add(k4ra).multiply(timestep/6);
 319                 deltaO = k2rv.add(k3rv).multiply(2).add(k1rv).add(k4rv).multiply(timestep/6);
 320
 321                 if (DEBUG)
 322                         System.out.println("Rot.Acc: "+deltaR+"  k1:"+k1ra+"  k2:"+k2ra+"  k3:"+k3ra+
 323                                         "  k4:"+k4ra);
 324
 325                 status.velocity = status.velocity.add(deltaV);
 326                 status.position = status.position.add(deltaP);
 327                 status.rotation = status.rotation.add(deltaR);
 328                 status.orientation.multiplyLeft(Quaternion.rotation(deltaO));
 329
 330
 331                 status.orientation.normalizeIfNecessary();
 332
 333                 status.time = status.time + timestep;
 334
 335
 336                 return null;
 337         }
 338
 339
 340
 341         /**
 342          * Calculate the linear and angular acceleration at the given status.  The results
 343          * are stored in the fields {@link #linearAcceleration} and {@link #angularAcceleration}.
 344          *
 345          * @param status   the status of the rocket.
 346          * @throws SimulationException
 347          */
 348         private void calculateAcceleration(RK4SimulationStatus status) throws SimulationException {
 349
 350                 /**
 351                  * Check whether to store warnings or not.  Warnings are ignored when on the
 352                  * launch rod or 0.25 seconds after departure, and when the velocity has dropped
 353                  * below 20% of the max. velocity.
 354                  */
 355                 WarningSet warnings = status.warnings;
 356                 maxVelocityZ = MathUtil.max(maxVelocityZ, status.velocity.z);
 357                 if (status.launchRod) {
 358                         warnings = null;
 359                 } else {
 360                         if (status.velocity.z < 0.2 * maxVelocityZ)
 361                                 warnings = null;
 362                         if (startWarningTime < 0)
 363                                 startWarningTime = status.time + 0.25;
 364                 }
 365                 if (status.time < startWarningTime)
 366                         warnings = null;
 367
 368
 369                 // Calculate aerodynamic forces  (only axial if still on launch rod)
 370                 calculator.setConfiguration(status.configuration);
 371
 372                 if (status.launchRod) {
 373                         forces = calculator.getAxialForces(status.time, flightConditions, warnings);
 374                 } else {
 375                         forces = calculator.getAerodynamicForces(status.time, flightConditions, warnings);
 376                 }
 377
 378
 379                 // Add very small randomization to yaw & pitch moments to prevent
 380                 // over-perfect flight
 381                 // TODO: HIGH: This should rather be performed as a listener
 382                 forces.Cm += PITCH_YAW_RANDOM * 2 * (random.nextDouble()-0.5);
 383                 forces.Cyaw += PITCH_YAW_RANDOM * 2 * (random.nextDouble()-0.5);
 384
 385
 386
 387
 388                 // Allow listeners to modify the forces
 389                 int mod = flightConditions.getModCount();
 390                 SimulationListener[] list = listeners.toArray(new SimulationListener[0]);
 391                 for (SimulationListener l: list) {
 392                         l.forceCalculation(status, flightConditions, forces);
 393                 }
 394                 if (flightConditions.getModCount() != mod) {
 395                         status.warnings.add(Warning.LISTENERS_AFFECTED);
 396                 }
 397
 398
 399
 400
 401
 402                 assert(!Double.isNaN(forces.CD));
 403                 assert(!Double.isNaN(forces.CN));
 404                 assert(!Double.isNaN(forces.Caxial));
 405                 assert(!Double.isNaN(forces.Cm));
 406                 assert(!Double.isNaN(forces.Cyaw));
 407                 assert(!Double.isNaN(forces.Cside));
 408                 assert(!Double.isNaN(forces.Croll));
 409
 410
 411                 ////////  Calculate forces and accelerations  ////////
 412
 413                 double dynP = (0.5 * flightConditions.getAtmosphericConditions().getDensity() *
 414                                 MathUtil.pow2(flightConditions.getVelocity()));
 415                 double refArea = flightConditions.getRefArea();
 416                 double refLength = flightConditions.getRefLength();
 417
 418
 419                 // Linear forces
 420                 thrustForce = calculateThrust(status, timestep);
 421                 dragForce = forces.Caxial * dynP * refArea;
 422                 double fN = forces.CN * dynP * refArea;
 423                 double fSide = forces.Cside * dynP * refArea;
 424
 425 //              double sin = Math.sin(flightConditions.getTheta());
 426 //              double cos = Math.cos(flightConditions.getTheta());
 427
 428 //              double forceX = - fN * cos - fSide * sin;
 429 //              double forceY = - fN * sin - fSide * cos;
 430                 double forceZ = thrustForce - dragForce;
 431
 432
 433 //              linearAcceleration = new Coordinate(forceX / forces.cg.weight,
 434 //                              forceY / forces.cg.weight, forceZ / forces.cg.weight);
 435                 linearAcceleration = new Coordinate(-fN / forces.cg.weight, -fSide / forces.cg.weight,
 436                                 forceZ / forces.cg.weight);
 437
 438                 linearAcceleration = thetaRotation.rotateZ(linearAcceleration);
 439                 linearAcceleration = status.orientation.rotate(linearAcceleration);
 440
 441                 linearAcceleration = linearAcceleration.sub(0, 0, status.gravityModel.getGravity());
 442
 443
 444                 // If still on launch rod, project acceleration onto launch rod direction and
 445                 // set angular acceleration to zero.
 446                 if (status.launchRod) {
 447                         linearAcceleration = status.launchRodDirection.multiply(
 448                                         linearAcceleration.dot(status.launchRodDirection));
 449                         angularAcceleration = Coordinate.NUL;
 450                         rollAcceleration = 0;
 451                         lateralPitchAcceleration = 0;
 452                         return;
 453                 }
 454
 455
 456                 // Convert momenta
 457                 double Cm = forces.Cm - forces.CN * forces.cg.x / refLength;
 458                 double Cyaw = forces.Cyaw - forces.Cside * forces.cg.x / refLength;
 459
 460 //              double momX = (-Cm * sin - Cyaw * cos) * dynP * refArea * refLength;
 461 //              double momY = ( Cm * cos - Cyaw * sin) * dynP * refArea * refLength;
 462                 double momX = -Cyaw * dynP * refArea * refLength;
 463                 double momY = Cm * dynP * refArea * refLength;
 464
 465                 double momZ = forces.Croll * dynP * refArea * refLength;
 466                 if (DEBUG)
 467                         System.out.printf("Croll:  %.3f  dynP=%.3f  momZ=%.3f\n",forces.Croll,dynP,momZ);
 468
 469                 assert(!Double.isNaN(momX));
 470                 assert(!Double.isNaN(momY));
 471                 assert(!Double.isNaN(momZ));
 472                 assert(!Double.isNaN(forces.longitudalInertia));
 473                 assert(!Double.isNaN(forces.rotationalInertia));
 474
 475                 angularAcceleration = new Coordinate(momX / forces.longitudalInertia,
 476                                 momY / forces.longitudalInertia, momZ / forces.rotationalInertia);
 477
 478                 rollAcceleration = angularAcceleration.z;
 479                 // TODO: LOW: This should be hypot, but does it matter?
 480                 lateralPitchAcceleration = MathUtil.max(Math.abs(angularAcceleration.x),
 481                                 Math.abs(angularAcceleration.y));
 482
 483                 if (DEBUG)
 484                         System.out.println("rot.inertia = "+forces.rotationalInertia);
 485
 486                 angularAcceleration = thetaRotation.rotateZ(angularAcceleration);
 487
 488                 angularAcceleration = status.orientation.rotate(angularAcceleration);
 489         }
 490
 491
 492         /**
 493          * Calculate the flight conditions for the current rocket status.  The conditions
 494          * are stored in the field {@link #flightConditions}.  Additional information that
 495          * is calculated and will be stored in the flight data is also computed into the
 496          * suitable fields.
 497          * @throws SimulationException
 498          */
 499         private void calculateFlightConditions(RK4SimulationStatus status) throws SimulationException {
 500
 501                 flightConditions.setReference(status.configuration);
 502
 503
 504                 //// Atmospheric conditions
 505                 AtmosphericConditions atmosphere = status.startConditions.getAtmosphericModel().
 506                         getConditions(status.position.z + status.startConditions.getLaunchAltitude());
 507                 flightConditions.setAtmosphericConditions(atmosphere);
 508
 509
 510                 //// Local wind speed and direction
 511                 windSpeed = status.windSimulator.getWindSpeed(status.time);
 512                 Coordinate airSpeed = status.velocity.add(windSpeed, 0, 0);
 513                 airSpeed = status.orientation.invRotate(airSpeed);
 514
 515
 516
 517         // Lateral direction:
 518         double len = MathUtil.hypot(airSpeed.x, airSpeed.y);
 519         if (len > 0.0001) {
 520             thetaRotation = new Rotation2D(airSpeed.y/len, airSpeed.x/len);
 521             flightConditions.setTheta(Math.atan2(airSpeed.y, airSpeed.x));
 522         } else {
 523             thetaRotation = Rotation2D.ID;
 524             flightConditions.setTheta(0);
 525         }
 526
 527                 double velocity = airSpeed.length();
 528         flightConditions.setVelocity(velocity);
 529         if (velocity > 0.01) {
 530             // aoa must be calculated from the monotonous cosine
 531             // sine can be calculated by a simple division
 532             flightConditions.setAOA(Math.acos(airSpeed.z / velocity), len / velocity);
 533         } else {
 534             flightConditions.setAOA(0);
 535         }
 536
 537
 538                 // Roll, pitch and yaw rate
 539                 Coordinate rot = status.orientation.invRotate(status.rotation);
 540                 rot = thetaRotation.invRotateZ(rot);
 541
 542                 flightConditions.setRollRate(rot.z);
 543                 if (len < 0.001) {
 544                         flightConditions.setPitchRate(0);
 545                         flightConditions.setYawRate(0);
 546                         lateralPitchRate = 0;
 547                 } else {
 548                         flightConditions.setPitchRate(rot.y);
 549                         flightConditions.setYawRate(rot.x);
 550                         // TODO: LOW: set this as power of two?
 551                         lateralPitchRate = MathUtil.hypot(rot.x, rot.y);
 552                 }
 553
 554
 555                 // Allow listeners to modify the conditions
 556                 int mod = flightConditions.getModCount();
 557                 SimulationListener[] list = listeners.toArray(new SimulationListener[0]);
 558                 for (SimulationListener l: list) {
 559                         l.flightConditions(status, flightConditions);
 560                 }
 561                 if (mod != flightConditions.getModCount()) {
 562                         // Re-calculate cached values
 563                         thetaRotation = new Rotation2D(flightConditions.getTheta());
 564                         lateralPitchRate = MathUtil.hypot(flightConditions.getPitchRate(),
 565                                         flightConditions.getYawRate());
 566                         status.warnings.add(Warning.LISTENERS_AFFECTED);
 567                 }
 568
 569         }
 570
 571
 572
 573         private void storeData(RK4SimulationStatus status) {
 574                 FlightDataBranch data = status.flightData;
 575                 boolean extra = status.startConditions.getCalculateExtras();
 576
 577                 data.addPoint();
 578                 data.setValue(FlightDataBranch.TYPE_TIME, status.time);
 579                 data.setValue(FlightDataBranch.TYPE_ALTITUDE, status.position.z);
 580                 data.setValue(FlightDataBranch.TYPE_POSITION_X, status.position.x);
 581                 data.setValue(FlightDataBranch.TYPE_POSITION_Y, status.position.y);
 582
 583                 if (extra) {
 584                         data.setValue(FlightDataBranch.TYPE_POSITION_XY,
 585                                         MathUtil.hypot(status.position.x, status.position.y));
 586                         data.setValue(FlightDataBranch.TYPE_POSITION_DIRECTION,
 587                                         Math.atan2(status.position.y, status.position.x));
 588
 589                         data.setValue(FlightDataBranch.TYPE_VELOCITY_XY,
 590                                         MathUtil.hypot(status.velocity.x, status.velocity.y));
 591                         data.setValue(FlightDataBranch.TYPE_ACCELERATION_XY,
 592                                         MathUtil.hypot(linearAcceleration.x, linearAcceleration.y));
 593
 594                         data.setValue(FlightDataBranch.TYPE_ACCELERATION_TOTAL,linearAcceleration.length());
 595
 596                         double Re = flightConditions.getVelocity() *
 597                                         calculator.getConfiguration().getLength() /
 598                                         flightConditions.getAtmosphericConditions().getKinematicViscosity();
 599                         data.setValue(FlightDataBranch.TYPE_REYNOLDS_NUMBER, Re);
 600                 }
 601
 602                 data.setValue(FlightDataBranch.TYPE_VELOCITY_Z, status.velocity.z);
 603                 data.setValue(FlightDataBranch.TYPE_ACCELERATION_Z, linearAcceleration.z);
 604
 605                 data.setValue(FlightDataBranch.TYPE_VELOCITY_TOTAL, flightConditions.getVelocity());
 606                 data.setValue(FlightDataBranch.TYPE_MACH_NUMBER, flightConditions.getMach());
 607
 608                 if (!status.launchRod) {
 609                         data.setValue(FlightDataBranch.TYPE_CP_LOCATION, forces.cp.x);
 610                         data.setValue(FlightDataBranch.TYPE_CG_LOCATION, forces.cg.x);
 611                         data.setValue(FlightDataBranch.TYPE_STABILITY,
 612                                         (forces.cp.x - forces.cg.x) / flightConditions.getRefLength());
 613                 }
 614                 data.setValue(FlightDataBranch.TYPE_MASS, forces.cg.weight);
 615
 616                 data.setValue(FlightDataBranch.TYPE_THRUST_FORCE, thrustForce);
 617                 data.setValue(FlightDataBranch.TYPE_DRAG_FORCE, dragForce);
 618
 619                 if (!status.launchRod) {
 620                         data.setValue(FlightDataBranch.TYPE_PITCH_MOMENT_COEFF,
 621                                         forces.Cm - forces.CN * forces.cg.x / flightConditions.getRefLength());
 622                         data.setValue(FlightDataBranch.TYPE_YAW_MOMENT_COEFF,
 623                                         forces.Cyaw - forces.Cside * forces.cg.x / flightConditions.getRefLength());
 624                         data.setValue(FlightDataBranch.TYPE_NORMAL_FORCE_COEFF, forces.CN);
 625                         data.setValue(FlightDataBranch.TYPE_SIDE_FORCE_COEFF, forces.Cside);
 626                         data.setValue(FlightDataBranch.TYPE_ROLL_MOMENT_COEFF, forces.Croll);
 627                         data.setValue(FlightDataBranch.TYPE_ROLL_FORCING_COEFF, forces.CrollForce);
 628                         data.setValue(FlightDataBranch.TYPE_ROLL_DAMPING_COEFF, forces.CrollDamp);
 629                         data.setValue(FlightDataBranch.TYPE_PITCH_DAMPING_MOMENT_COEFF,
 630                                         forces.pitchDampingMoment);
 631                 }
 632
 633                 data.setValue(FlightDataBranch.TYPE_DRAG_COEFF, forces.CD);
 634                 data.setValue(FlightDataBranch.TYPE_AXIAL_DRAG_COEFF, forces.Caxial);
 635                 data.setValue(FlightDataBranch.TYPE_FRICTION_DRAG_COEFF, forces.frictionCD);
 636                 data.setValue(FlightDataBranch.TYPE_PRESSURE_DRAG_COEFF, forces.pressureCD);
 637                 data.setValue(FlightDataBranch.TYPE_BASE_DRAG_COEFF, forces.baseCD);
 638
 639                 data.setValue(FlightDataBranch.TYPE_REFERENCE_LENGTH, flightConditions.getRefLength());
 640                 data.setValue(FlightDataBranch.TYPE_REFERENCE_AREA, flightConditions.getRefArea());
 641
 642
 643                 data.setValue(FlightDataBranch.TYPE_PITCH_RATE, flightConditions.getPitchRate());
 644                 data.setValue(FlightDataBranch.TYPE_YAW_RATE, flightConditions.getYawRate());
 645
 646
 647
 648                 if (extra) {
 649                         Coordinate c = status.orientation.rotateZ();
 650                         double theta = Math.atan2(c.z, MathUtil.hypot(c.x, c.y));
 651                         double phi = Math.atan2(c.y, c.x);
 652                         if (phi < -(Math.PI-0.0001))
 653                                 phi = Math.PI;
 654                         data.setValue(FlightDataBranch.TYPE_ORIENTATION_THETA, theta);
 655                         data.setValue(FlightDataBranch.TYPE_ORIENTATION_PHI, phi);
 656                 }
 657
 658                 data.setValue(FlightDataBranch.TYPE_AOA, flightConditions.getAOA());
 659                 data.setValue(FlightDataBranch.TYPE_ROLL_RATE, flightConditions.getRollRate());
 660
 661                 data.setValue(FlightDataBranch.TYPE_WIND_VELOCITY, windSpeed);
 662                 data.setValue(FlightDataBranch.TYPE_AIR_TEMPERATURE,
 663                                 flightConditions.getAtmosphericConditions().temperature);
 664                 data.setValue(FlightDataBranch.TYPE_AIR_PRESSURE,
 665                                 flightConditions.getAtmosphericConditions().pressure);
 666                 data.setValue(FlightDataBranch.TYPE_SPEED_OF_SOUND,
 667                                 flightConditions.getAtmosphericConditions().getMachSpeed());
 668
 669
 670                 data.setValue(FlightDataBranch.TYPE_TIME_STEP, timestep);
 671                 data.setValue(FlightDataBranch.TYPE_COMPUTATION_TIME,
 672                                 (System.nanoTime() - status.simulationStartTime)/1000000000.0);
 673
 674
 675 //              data.setValue(FlightDataBranch.TYPE_, 0);
 676
 677         }
 678
 679
 680 }