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