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