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

   1 package net.sf.openrocket.simulation;
   2
   3 import java.util.HashSet;
   4 import java.util.Iterator;
   5 import java.util.Set;
   6
   7 import net.sf.openrocket.aerodynamics.FlightConditions;
   8 import net.sf.openrocket.aerodynamics.Warning;
   9 import net.sf.openrocket.aerodynamics.WarningSet;
  10 import net.sf.openrocket.logging.LogHelper;
  11 import net.sf.openrocket.motor.Motor;
  12 import net.sf.openrocket.motor.MotorId;
  13 import net.sf.openrocket.motor.MotorInstance;
  14 import net.sf.openrocket.motor.MotorInstanceConfiguration;
  15 import net.sf.openrocket.rocketcomponent.Configuration;
  16 import net.sf.openrocket.rocketcomponent.LaunchLug;
  17 import net.sf.openrocket.rocketcomponent.MotorMount;
  18 import net.sf.openrocket.rocketcomponent.RecoveryDevice;
  19 import net.sf.openrocket.rocketcomponent.RocketComponent;
  20 import net.sf.openrocket.simulation.exception.SimulationException;
  21 import net.sf.openrocket.simulation.exception.SimulationLaunchException;
  22 import net.sf.openrocket.simulation.listeners.SimulationListenerHelper;
  23 import net.sf.openrocket.startup.Application;
  24 import net.sf.openrocket.unit.UnitGroup;
  25 import net.sf.openrocket.util.Coordinate;
  26 import net.sf.openrocket.util.MathUtil;
  27 import net.sf.openrocket.util.Pair;
  28 import net.sf.openrocket.util.Quaternion;
  29
  30
  31 public class BasicEventSimulationEngine implements SimulationEngine {
  32
  33         private static final LogHelper log = Application.getLogger();
  34
  35         // TODO: HIGH: Allow selecting steppers
  36         private SimulationStepper flightStepper = new RK4SimulationStepper();
  37         private SimulationStepper landingStepper = new BasicLandingStepper();
  38
  39         private SimulationStepper currentStepper;
  40
  41         private SimulationStatus status;
  42
  43
  44         @Override
  45         public FlightData simulate(SimulationConditions simulationConditions) throws SimulationException {
  46                 Set<MotorId> motorBurntOut = new HashSet<MotorId>();
  47
  48                 // Set up flight data
  49                 FlightData flightData = new FlightData();
  50
  51                 // Set up rocket configuration
  52                 Configuration configuration = setupConfiguration(simulationConditions);
  53                 MotorInstanceConfiguration motorConfiguration = setupMotorConfiguration(configuration);
  54                 if (motorConfiguration.getMotorIDs().isEmpty()) {
  55                         throw new SimulationLaunchException("No motors defined.");
  56                 }
  57
  58                 // Initialize the simulation
  59                 currentStepper = flightStepper;
  60                 status = initialStatus(configuration, motorConfiguration, simulationConditions);
  61                 status = currentStepper.initialize(status);
  62
  63
  64                 SimulationListenerHelper.fireStartSimulation(status);
  65
  66                 try {
  67
  68                         // Start the simulation
  69                         while (handleEvents()) {
  70
  71                                 // Take the step
  72                                 double oldAlt = status.getRocketPosition().z;
  73
  74                                 if (SimulationListenerHelper.firePreStep(status)) {
  75                                         // Step at most to the next event
  76                                         double maxStepTime = Double.MAX_VALUE;
  77                                         FlightEvent nextEvent = status.getEventQueue().peek();
  78                                         if (nextEvent != null) {
  79                                                 maxStepTime = MathUtil.max(nextEvent.getTime() - status.getSimulationTime(), 0.001);
  80                                         }
  81                                         log.verbose("BasicEventSimulationEngine: Taking simulation step at t=" + status.getSimulationTime());
  82                                         currentStepper.step(status, maxStepTime);
  83                                 }
  84                                 SimulationListenerHelper.firePostStep(status);
  85
  86
  87                                 // Check for NaN values in the simulation status
  88                                 checkNaN();
  89
  90                                 // Add altitude event
  91                                 addEvent(new FlightEvent(FlightEvent.Type.ALTITUDE, status.getSimulationTime(),
  92                                                 status.getConfiguration().getRocket(),
  93                                                 new Pair<Double, Double>(oldAlt, status.getRocketPosition().z)));
  94
  95
  96                                 // Add appropriate events
  97                                 if (!status.isLiftoff()) {
  98
  99                                         // Avoid sinking into ground before liftoff
 100                                         if (status.getRocketPosition().z < 0) {
 101                                                 status.setRocketPosition(Coordinate.NUL);
 102                                                 status.setRocketVelocity(Coordinate.NUL);
 103                                         }
 104                                         // Detect lift-off
 105                                         if (status.getRocketPosition().z > 0.01) {
 106                                                 addEvent(new FlightEvent(FlightEvent.Type.LIFTOFF, status.getSimulationTime()));
 107                                         }
 108
 109                                 } else {
 110
 111                                         // Check ground hit after liftoff
 112                                         if (status.getRocketPosition().z < 0) {
 113                                                 status.setRocketPosition(status.getRocketPosition().setZ(0));
 114                                                 addEvent(new FlightEvent(FlightEvent.Type.GROUND_HIT, status.getSimulationTime()));
 115                                                 addEvent(new FlightEvent(FlightEvent.Type.SIMULATION_END, status.getSimulationTime()));
 116                                         }
 117
 118                                 }
 119
 120                                 // Check for launch guide clearance
 121                                 if (!status.isLaunchRodCleared() &&
 122                                                 status.getRocketPosition().length() > status.getSimulationConditions().getLaunchRodLength()) {
 123                                         addEvent(new FlightEvent(FlightEvent.Type.LAUNCHROD, status.getSimulationTime(), null));
 124                                 }
 125
 126
 127                                 // Check for apogee
 128                                 if (!status.isApogeeReached() && status.getRocketPosition().z < oldAlt - 0.001) {
 129                                         addEvent(new FlightEvent(FlightEvent.Type.APOGEE, status.getSimulationTime(),
 130                                                         status.getConfiguration().getRocket()));
 131                                 }
 132
 133
 134                                 // Check for burnt out motors
 135                                 for (MotorId motorId : status.getMotorConfiguration().getMotorIDs()) {
 136                                         MotorInstance motor = status.getMotorConfiguration().getMotorInstance(motorId);
 137                                         if (!motor.isActive() && motorBurntOut.add(motorId)) {
 138                                                 addEvent(new FlightEvent(FlightEvent.Type.BURNOUT, status.getSimulationTime(),
 139                                                                 (RocketComponent) status.getMotorConfiguration().getMotorMount(motorId), motorId));
 140                                         }
 141                                 }
 142
 143                         }
 144
 145                 } catch (SimulationException e) {
 146                         SimulationListenerHelper.fireEndSimulation(status, e);
 147                         throw e;
 148                 }
 149
 150                 SimulationListenerHelper.fireEndSimulation(status, null);
 151
 152                 flightData.addBranch(status.getFlightData());
 153
 154                 log.info("Warnings at the end of simulation:  " + flightData.getWarningSet());
 155
 156                 // TODO: HIGH: Simulate branches
 157                 return flightData;
 158         }
 159
 160
 161
 162         private SimulationStatus initialStatus(Configuration configuration,
 163                         MotorInstanceConfiguration motorConfiguration,
 164                         SimulationConditions simulationConditions) {
 165
 166                 SimulationStatus init = new SimulationStatus();
 167                 init.setSimulationConditions(simulationConditions);
 168                 init.setConfiguration(configuration);
 169                 init.setMotorConfiguration(motorConfiguration);
 170
 171                 init.setSimulationTime(0);
 172                 init.setPreviousTimeStep(simulationConditions.getTimeStep());
 173                 init.setRocketPosition(Coordinate.NUL);
 174                 init.setRocketVelocity(Coordinate.NUL);
 175
 176
 177                 // Initialize to roll angle with least stability w.r.t. the wind
 178                 Quaternion o;
 179                 FlightConditions cond = new FlightConditions(configuration);
 180                 simulationConditions.getAerodynamicCalculator().getWorstCP(configuration, cond, null);
 181                 double angle = -cond.getTheta() - simulationConditions.getLaunchRodDirection();
 182                 o = Quaternion.rotation(new Coordinate(0, 0, angle));
 183
 184                 // Launch rod angle and direction
 185                 o = o.multiplyLeft(Quaternion.rotation(new Coordinate(0, simulationConditions.getLaunchRodAngle(), 0)));
 186                 o = o.multiplyLeft(Quaternion.rotation(new Coordinate(0, 0, simulationConditions.getLaunchRodDirection())));
 187
 188                 init.setRocketOrientationQuaternion(o);
 189                 init.setRocketRotationVelocity(Coordinate.NUL);
 190
 191
 192                 /*
 193                  * Calculate the effective launch rod length taking into account launch lugs.
 194                  * If no lugs are found, assume a tower launcher of full length.
 195                  */
 196                 double length = simulationConditions.getLaunchRodLength();
 197                 double lugPosition = Double.NaN;
 198                 for (RocketComponent c : configuration) {
 199                         if (c instanceof LaunchLug) {
 200                                 double pos = c.toAbsolute(new Coordinate(c.getLength()))[0].x;
 201                                 if (Double.isNaN(lugPosition) || pos > lugPosition) {
 202                                         lugPosition = pos;
 203                                 }
 204                         }
 205                 }
 206                 if (!Double.isNaN(lugPosition)) {
 207                         double maxX = 0;
 208                         for (Coordinate c : configuration.getBounds()) {
 209                                 if (c.x > maxX)
 210                                         maxX = c.x;
 211                         }
 212                         if (maxX >= lugPosition) {
 213                                 length = Math.max(0, length - (maxX - lugPosition));
 214                         }
 215                 }
 216                 init.setEffectiveLaunchRodLength(length);
 217
 218
 219
 220                 init.setSimulationStartWallTime(System.nanoTime());
 221
 222                 init.setMotorIgnited(false);
 223                 init.setLiftoff(false);
 224                 init.setLaunchRodCleared(false);
 225                 init.setApogeeReached(false);
 226
 227                 init.getEventQueue().add(new FlightEvent(FlightEvent.Type.LAUNCH, 0, simulationConditions.getRocket()));
 228
 229                 init.setFlightData(new FlightDataBranch("MAIN", FlightDataType.TYPE_TIME));
 230                 init.setWarnings(new WarningSet());
 231
 232                 return init;
 233         }
 234
 235
 236
 237         /**
 238          * Create a rocket configuration from the launch conditions.
 239          *
 240          * @param simulation    the launch conditions.
 241          * @return                              a rocket configuration with all stages attached.
 242          */
 243         private Configuration setupConfiguration(SimulationConditions simulation) {
 244                 Configuration configuration = new Configuration(simulation.getRocket());
 245                 configuration.setAllStages();
 246                 configuration.setMotorConfigurationID(simulation.getMotorConfigurationID());
 247
 248                 return configuration;
 249         }
 250
 251
 252
 253         /**
 254          * Create a new motor instance configuration for the rocket configuration.
 255          *
 256          * @param configuration         the rocket configuration.
 257          * @return                                      a new motor instance configuration with all motors in place.
 258          */
 259         private MotorInstanceConfiguration setupMotorConfiguration(Configuration configuration) {
 260                 MotorInstanceConfiguration motors = new MotorInstanceConfiguration();
 261                 final String motorId = configuration.getMotorConfigurationID();
 262
 263                 Iterator<MotorMount> iterator = configuration.motorIterator();
 264                 while (iterator.hasNext()) {
 265                         MotorMount mount = iterator.next();
 266                         RocketComponent component = (RocketComponent) mount;
 267                         Motor motor = mount.getMotor(motorId);
 268
 269                         if (motor != null) {
 270                                 Coordinate[] positions = component.toAbsolute(mount.getMotorPosition(motorId));
 271                                 for (int i = 0; i < positions.length; i++) {
 272                                         Coordinate position = positions[i];
 273                                         MotorId id = new MotorId(component.getID(), i + 1);
 274                                         motors.addMotor(id, motor.getInstance(), mount, position);
 275                                 }
 276                         }
 277                 }
 278                 return motors;
 279         }
 280
 281         /**
 282          * Handles events occurring during the flight from the event queue.
 283          * Each event that has occurred before or at the current simulation time is
 284          * processed.  Suitable events are also added to the flight data.
 285          */
 286         private boolean handleEvents() throws SimulationException {
 287                 boolean ret = true;
 288                 FlightEvent event;
 289
 290                 for (event = nextEvent(); event != null; event = nextEvent()) {
 291
 292                         // Call simulation listeners, allow aborting event handling
 293                         if (!SimulationListenerHelper.fireHandleFlightEvent(status, event)) {
 294                                 continue;
 295                         }
 296
 297                         if (event.getType() != FlightEvent.Type.ALTITUDE) {
 298                                 log.verbose("BasicEventSimulationEngine:  Handling event " + event);
 299                         }
 300
 301                         if (event.getType() == FlightEvent.Type.IGNITION) {
 302                                 MotorMount mount = (MotorMount) event.getSource();
 303                                 MotorId motorId = (MotorId) event.getData();
 304                                 MotorInstance instance = status.getMotorConfiguration().getMotorInstance(motorId);
 305                                 if (!SimulationListenerHelper.fireMotorIgnition(status, motorId, mount, instance)) {
 306                                         continue;
 307                                 }
 308                         }
 309
 310                         if (event.getType() == FlightEvent.Type.RECOVERY_DEVICE_DEPLOYMENT) {
 311                                 RecoveryDevice device = (RecoveryDevice) event.getSource();
 312                                 if (!SimulationListenerHelper.fireRecoveryDeviceDeployment(status, device)) {
 313                                         continue;
 314                                 }
 315                         }
 316
 317
 318
 319                         // Check for motor ignition events, add ignition events to queue
 320                         for (MotorId id : status.getMotorConfiguration().getMotorIDs()) {
 321                                 MotorMount mount = status.getMotorConfiguration().getMotorMount(id);
 322                                 RocketComponent component = (RocketComponent) mount;
 323
 324                                 if (mount.getIgnitionEvent().isActivationEvent(event, component)) {
 325                                         addEvent(new FlightEvent(FlightEvent.Type.IGNITION,
 326                                                         status.getSimulationTime() + mount.getIgnitionDelay(),
 327                                                         component, id));
 328                                 }
 329                         }
 330
 331
 332                         // Check for recovery device deployment, add events to queue
 333                         Iterator<RocketComponent> rci = status.getConfiguration().iterator();
 334                         while (rci.hasNext()) {
 335                                 RocketComponent c = rci.next();
 336                                 if (!(c instanceof RecoveryDevice))
 337                                         continue;
 338                                 if (((RecoveryDevice) c).getDeployEvent().isActivationEvent(event, c)) {
 339                                         // Delay event by at least 1ms to allow stage separation to occur first
 340                                         addEvent(new FlightEvent(FlightEvent.Type.RECOVERY_DEVICE_DEPLOYMENT,
 341                                                         event.getTime() + Math.max(0.001, ((RecoveryDevice) c).getDeployDelay()), c));
 342                                 }
 343                         }
 344
 345
 346                         // Handle event
 347                         switch (event.getType()) {
 348
 349                         case LAUNCH: {
 350                                 status.getFlightData().addEvent(event);
 351                                 break;
 352                         }
 353
 354                         case IGNITION: {
 355                                 // Ignite the motor
 356                                 MotorMount mount = (MotorMount) event.getSource();
 357                                 RocketComponent component = (RocketComponent) mount;
 358                                 MotorId motorId = (MotorId) event.getData();
 359                                 MotorInstanceConfiguration config = status.getMotorConfiguration();
 360                                 config.setMotorIgnitionTime(motorId, event.getTime());
 361                                 status.setMotorIgnited(true);
 362                                 status.getFlightData().addEvent(event);
 363
 364                                 // Add stage separation event if appropriate
 365                                 int n = component.getStageNumber();
 366                                 if (n < component.getRocket().getStageCount() - 1) {
 367                                         if (status.getConfiguration().isStageActive(n + 1)) {
 368                                                 addEvent(new FlightEvent(FlightEvent.Type.STAGE_SEPARATION, event.getTime(),
 369                                                                 component.getStage()));
 370                                         }
 371                                 }
 372                                 break;
 373                         }
 374
 375                         case LIFTOFF: {
 376                                 // Mark lift-off as occurred
 377                                 status.setLiftoff(true);
 378                                 status.getFlightData().addEvent(event);
 379                                 break;
 380                         }
 381
 382                         case LAUNCHROD: {
 383                                 // Mark launch rod as cleared
 384                                 status.setLaunchRodCleared(true);
 385                                 status.getFlightData().addEvent(event);
 386                                 break;
 387                         }
 388
 389                         case BURNOUT: {
 390                                 // If motor burnout occurs without lift-off, abort
 391                                 if (!status.isLiftoff()) {
 392                                         throw new SimulationLaunchException("Motor burnout without liftoff.");
 393                                 }
 394                                 // Add ejection charge event
 395                                 String id = status.getConfiguration().getMotorConfigurationID();
 396                                 MotorMount mount = (MotorMount) event.getSource();
 397                                 double delay = mount.getMotorDelay(id);
 398                                 if (delay != Motor.PLUGGED) {
 399                                         addEvent(new FlightEvent(FlightEvent.Type.EJECTION_CHARGE, status.getSimulationTime() + delay,
 400                                                         event.getSource(), event.getData()));
 401                                 }
 402                                 status.getFlightData().addEvent(event);
 403                                 break;
 404                         }
 405
 406                         case EJECTION_CHARGE: {
 407                                 status.getFlightData().addEvent(event);
 408                                 break;
 409                         }
 410
 411                         case STAGE_SEPARATION: {
 412                                 // TODO: HIGH: Store lower stages to be simulated later
 413                                 RocketComponent stage = event.getSource();
 414                                 int n = stage.getStageNumber();
 415                                 status.getConfiguration().setToStage(n);
 416                                 status.getFlightData().addEvent(event);
 417                                 break;
 418                         }
 419
 420                         case APOGEE:
 421                                 // Mark apogee as reached
 422                                 status.setApogeeReached(true);
 423                                 status.getFlightData().addEvent(event);
 424                                 break;
 425
 426                         case RECOVERY_DEVICE_DEPLOYMENT:
 427                                 RocketComponent c = event.getSource();
 428                                 int n = c.getStageNumber();
 429                                 // Ignore event if stage not active
 430                                 if (status.getConfiguration().isStageActive(n)) {
 431                                         // TODO: HIGH: Check stage activeness for other events as well?
 432
 433                                         // Check whether any motor in the active stages is active anymore
 434                                         for (MotorId motorId : status.getMotorConfiguration().getMotorIDs()) {
 435                                                 int stage = ((RocketComponent) status.getMotorConfiguration().
 436                                                                 getMotorMount(motorId)).getStageNumber();
 437                                                 if (!status.getConfiguration().isStageActive(stage))
 438                                                         continue;
 439                                                 if (!status.getMotorConfiguration().getMotorInstance(motorId).isActive())
 440                                                         continue;
 441                                                 status.getWarnings().add(Warning.RECOVERY_DEPLOYMENT_WHILE_BURNING);
 442                                         }
 443
 444                                         // Check for launch rod
 445                                         if (!status.isLaunchRodCleared()) {
 446                                                 status.getWarnings().add(Warning.fromString("Recovery device device deployed while on " +
 447                                                                 "the launch guide."));
 448                                         }
 449
 450                                         // Check current velocity
 451                                         if (status.getRocketVelocity().length() > 20) {
 452                                                 // TODO: LOW: Custom warning.
 453                                                 status.getWarnings().add(Warning.fromString("Recovery device deployment at high " +
 454                                                                 "speed ("
 455                                                                 + UnitGroup.UNITS_VELOCITY.toStringUnit(status.getRocketVelocity().length())
 456                                                                 + ")."));
 457                                         }
 458
 459                                         status.setLiftoff(true);
 460                                         status.getDeployedRecoveryDevices().add((RecoveryDevice) c);
 461
 462                                         this.currentStepper = this.landingStepper;
 463                                         this.status = currentStepper.initialize(status);
 464
 465                                         status.getFlightData().addEvent(event);
 466                                 }
 467                                 break;
 468
 469                         case GROUND_HIT:
 470                                 status.getFlightData().addEvent(event);
 471                                 break;
 472
 473                         case SIMULATION_END:
 474                                 ret = false;
 475                                 status.getFlightData().addEvent(event);
 476                                 break;
 477
 478                         case ALTITUDE:
 479                                 break;
 480                         }
 481
 482                 }
 483
 484
 485                 // If no motor has ignited, abort
 486                 if (!status.isMotorIgnited()) {
 487                         throw new SimulationLaunchException("No motors ignited.");
 488                 }
 489
 490                 return ret;
 491         }
 492
 493
 494         /**
 495          * Add a flight event to the event queue unless a listener aborts adding it.
 496          *
 497          * @param event         the event to add to the queue.
 498          */
 499         private void addEvent(FlightEvent event) throws SimulationException {
 500                 if (SimulationListenerHelper.fireAddFlightEvent(status, event)) {
 501                         status.getEventQueue().add(event);
 502                 }
 503         }
 504
 505
 506
 507         /**
 508          * Return the next flight event to handle, or null if no more events should be handled.
 509          * This method jumps the simulation time forward in case no motors have been ignited.
 510          * The flight event is removed from the event queue.
 511          *
 512          * @param status        the simulation status
 513          * @return                      the flight event to handle, or null
 514          */
 515         private FlightEvent nextEvent() {
 516                 EventQueue queue = status.getEventQueue();
 517                 FlightEvent event = queue.peek();
 518                 if (event == null)
 519                         return null;
 520
 521                 // Jump to event if no motors have been ignited
 522                 if (!status.isMotorIgnited() && event.getTime() > status.getSimulationTime()) {
 523                         status.setSimulationTime(event.getTime());
 524                 }
 525                 if (event.getTime() <= status.getSimulationTime()) {
 526                         return queue.poll();
 527                 } else {
 528                         return null;
 529                 }
 530         }
 531
 532
 533
 534         private void checkNaN() throws SimulationException {
 535                 double d = 0;
 536                 boolean b = false;
 537                 d += status.getSimulationTime();
 538                 d += status.getPreviousTimeStep();
 539                 b |= status.getRocketPosition().isNaN();
 540                 b |= status.getRocketVelocity().isNaN();
 541                 b |= status.getRocketOrientationQuaternion().isNaN();
 542                 b |= status.getRocketRotationVelocity().isNaN();
 543                 d += status.getEffectiveLaunchRodLength();
 544
 545                 if (Double.isNaN(d) || b) {
 546                         log.error("Simulation resulted in NaN value:" +
 547                                         " simulationTime=" + status.getSimulationTime() +
 548                                         " previousTimeStep=" + status.getPreviousTimeStep() +
 549                                         " rocketPosition=" + status.getRocketPosition() +
 550                                         " rocketVelocity=" + status.getRocketVelocity() +
 551                                         " rocketOrientationQuaternion=" + status.getRocketOrientationQuaternion() +
 552                                         " rocketRotationVelocity=" + status.getRocketRotationVelocity() +
 553                                         " effectiveLaunchRodLength=" + status.getEffectiveLaunchRodLength());
 554                         throw new SimulationException("Simulation resulted in not-a-number (NaN) value, please report a bug.");
 555                 }
 556         }
 557
 558
 559 }