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