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