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