1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23 package org.hipparchus.ode;
24
25 import java.util.ArrayList;
26 import java.util.Collections;
27 import java.util.Comparator;
28 import java.util.List;
29 import java.util.PriorityQueue;
30 import java.util.Queue;
31 import java.util.stream.Collectors;
32 import java.util.stream.Stream;
33
34 import org.hipparchus.CalculusFieldElement;
35 import org.hipparchus.Field;
36 import org.hipparchus.exception.MathIllegalArgumentException;
37 import org.hipparchus.exception.MathIllegalStateException;
38 import org.hipparchus.ode.events.Action;
39 import org.hipparchus.ode.events.FieldDetectorBasedEventState;
40 import org.hipparchus.ode.events.FieldEventOccurrence;
41 import org.hipparchus.ode.events.FieldEventState;
42 import org.hipparchus.ode.events.FieldODEEventDetector;
43 import org.hipparchus.ode.events.FieldODEStepEndHandler;
44 import org.hipparchus.ode.events.FieldStepEndEventState;
45 import org.hipparchus.ode.sampling.AbstractFieldODEStateInterpolator;
46 import org.hipparchus.ode.sampling.FieldODEStepHandler;
47 import org.hipparchus.util.FastMath;
48 import org.hipparchus.util.Incrementor;
49
50
51
52
53
54 public abstract class AbstractFieldIntegrator<T extends CalculusFieldElement<T>> implements FieldODEIntegrator<T> {
55
56
57 private final List<FieldODEStepHandler<T>> stepHandlers;
58
59
60 private FieldODEStateAndDerivative<T> stepStart;
61
62
63 private T stepSize;
64
65
66 private boolean isLastStep;
67
68
69 private boolean resetOccurred;
70
71
72 private final Field<T> field;
73
74
75 private final List<FieldDetectorBasedEventState<T>> detectorBasedEventsStates;
76
77
78 private final List<FieldStepEndEventState<T>> stepEndEventsStates;
79
80
81 private boolean statesInitialized;
82
83
84 private final String name;
85
86
87 private Incrementor evaluations;
88
89
90 private transient FieldExpandableODE<T> equations;
91
92
93
94
95
96 protected AbstractFieldIntegrator(final Field<T> field, final String name) {
97 this.field = field;
98 this.name = name;
99 stepHandlers = new ArrayList<>();
100 stepStart = null;
101 stepSize = null;
102 detectorBasedEventsStates = new ArrayList<>();
103 stepEndEventsStates = new ArrayList<>();
104 statesInitialized = false;
105 evaluations = new Incrementor();
106 }
107
108
109
110
111 public Field<T> getField() {
112 return field;
113 }
114
115
116 @Override
117 public String getName() {
118 return name;
119 }
120
121
122 @Override
123 public void addStepHandler(final FieldODEStepHandler<T> handler) {
124 stepHandlers.add(handler);
125 }
126
127
128 @Override
129 public List<FieldODEStepHandler<T>> getStepHandlers() {
130 return Collections.unmodifiableList(stepHandlers);
131 }
132
133
134 @Override
135 public void clearStepHandlers() {
136 stepHandlers.clear();
137 }
138
139
140 @Override
141 public void addEventDetector(final FieldODEEventDetector<T> detector) {
142 detectorBasedEventsStates.add(new FieldDetectorBasedEventState<>(detector));
143 }
144
145
146 @Override
147 public List<FieldODEEventDetector<T>> getEventDetectors() {
148 return detectorBasedEventsStates.stream().map(es -> es.getEventDetector()).collect(Collectors.toList());
149 }
150
151
152 @Override
153 public void clearEventDetectors() {
154 detectorBasedEventsStates.clear();
155 }
156
157
158 @Override
159 public void addStepEndHandler(FieldODEStepEndHandler<T> handler) {
160 stepEndEventsStates.add(new FieldStepEndEventState<>(handler));
161 }
162
163
164 @Override
165 public List<FieldODEStepEndHandler<T>> getStepEndHandlers() {
166 return stepEndEventsStates.stream().map(es -> es.getHandler()).collect(Collectors.toList());
167 }
168
169
170 @Override
171 public void clearStepEndHandlers() {
172 stepEndEventsStates.clear();
173 }
174
175
176 @Override
177 public T getCurrentSignedStepsize() {
178 return stepSize;
179 }
180
181
182 @Override
183 public void setMaxEvaluations(int maxEvaluations) {
184 evaluations = evaluations.withMaximalCount((maxEvaluations < 0) ? Integer.MAX_VALUE : maxEvaluations);
185 }
186
187
188 @Override
189 public int getMaxEvaluations() {
190 return evaluations.getMaximalCount();
191 }
192
193
194 @Override
195 public int getEvaluations() {
196 return evaluations.getCount();
197 }
198
199
200
201
202
203
204
205 protected FieldODEStateAndDerivative<T> initIntegration(final FieldExpandableODE<T> eqn,
206 final FieldODEState<T> s0, final T t) {
207
208 this.equations = eqn;
209 evaluations = evaluations.withCount(0);
210
211
212 eqn.init(s0, t);
213
214
215 final T t0 = s0.getTime();
216 final T[] y0 = s0.getCompleteState();
217 final T[] y0Dot = computeDerivatives(t0, y0);
218
219
220 final FieldODEStateAndDerivative<T> s0WithDerivatives =
221 eqn.getMapper().mapStateAndDerivative(t0, y0, y0Dot);
222
223
224 detectorBasedEventsStates.stream().forEach(s -> {
225 s.init(s0WithDerivatives, t);
226 s.getEventDetector().getHandler().init(s0WithDerivatives, t, s.getEventDetector());
227 });
228
229
230 stepEndEventsStates.stream().forEach(s -> {
231 s.init(s0WithDerivatives, t);
232 s.getHandler().init(s0WithDerivatives, t);
233 });
234
235
236 for (FieldODEStepHandler<T> handler : stepHandlers) {
237 handler.init(s0WithDerivatives, t);
238 }
239
240 setStateInitialized(false);
241
242 return s0WithDerivatives;
243
244 }
245
246
247
248
249 protected FieldExpandableODE<T> getEquations() {
250 return equations;
251 }
252
253
254
255
256 protected Incrementor getEvaluationsCounter() {
257 return evaluations;
258 }
259
260
261
262
263
264
265
266
267
268
269
270 public T[] computeDerivatives(final T t, final T[] y)
271 throws MathIllegalArgumentException, MathIllegalStateException, NullPointerException {
272 evaluations.increment();
273 return equations.computeDerivatives(t, y);
274 }
275
276
277
278
279
280 protected void incrementEvaluations(final int nTimes) {
281 evaluations.increment(nTimes);
282 }
283
284
285
286
287
288
289
290 protected void setStateInitialized(final boolean stateInitialized) {
291 this.statesInitialized = stateInitialized;
292 }
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307 protected FieldODEStateAndDerivative<T> acceptStep(final AbstractFieldODEStateInterpolator<T> interpolator,
308 final T tEnd)
309 throws MathIllegalArgumentException, MathIllegalStateException {
310
311 FieldODEStateAndDerivative<T> previousState = interpolator.getGlobalPreviousState();
312 final FieldODEStateAndDerivative<T> currentState = interpolator.getGlobalCurrentState();
313 AbstractFieldODEStateInterpolator<T> restricted = interpolator;
314
315
316 if (!statesInitialized) {
317
318 detectorBasedEventsStates.stream().forEach(s -> s.reinitializeBegin(interpolator));
319 statesInitialized = true;
320 }
321
322
323 stepEndEventsStates.stream().forEach(s -> s.setStepEnd(currentState.getTime()));
324
325
326 final int orderingSign = interpolator.isForward() ? +1 : -1;
327 final Queue<FieldEventState<T>> occurringEvents = new PriorityQueue<>(new Comparator<FieldEventState<T>>() {
328
329 @Override
330 public int compare(FieldEventState<T> es0, FieldEventState<T> es1) {
331 return orderingSign * Double.compare(es0.getEventTime().getReal(), es1.getEventTime().getReal());
332 }
333 });
334
335 resetOccurred = false;
336 boolean doneWithStep = false;
337 resetEvents:
338 do {
339
340
341 occurringEvents.clear();
342 final AbstractFieldODEStateInterpolator<T> finalRestricted = restricted;
343 Stream.concat(detectorBasedEventsStates.stream(), stepEndEventsStates.stream()).
344 forEach(s -> { if (s.evaluateStep(finalRestricted)) {
345
346 occurringEvents.add(s);
347 }
348 });
349
350
351 do {
352
353 eventLoop:
354 while (!occurringEvents.isEmpty()) {
355
356
357 final FieldEventState<T> currentEvent = occurringEvents.poll();
358
359
360 FieldODEStateAndDerivative<T> eventState =
361 restricted.getInterpolatedState(currentEvent.getEventTime());
362
363
364 restricted = restricted.restrictStep(previousState, eventState);
365
366
367 for (final FieldDetectorBasedEventState<T> state : detectorBasedEventsStates) {
368 if (state != currentEvent && state.tryAdvance(eventState, interpolator)) {
369
370
371 occurringEvents.remove(state);
372
373 occurringEvents.add(state);
374
375 occurringEvents.add(currentEvent);
376 continue eventLoop;
377 }
378 }
379
380
381
382 for (final FieldODEStepHandler<T> handler : stepHandlers) {
383 handler.handleStep(restricted);
384 }
385
386
387 final FieldEventOccurrence<T> occurrence = currentEvent.doEvent(eventState);
388 final Action action = occurrence.getAction();
389 isLastStep = action == Action.STOP;
390
391 if (isLastStep) {
392
393
394
395
396 final FieldODEStateAndDerivative<T> savedState = eventState;
397 eventState = interpolator.getInterpolatedState(occurrence.getStopTime());
398 restricted = interpolator.restrictStep(savedState, eventState);
399
400
401 for (final FieldODEStepHandler<T> handler : stepHandlers) {
402 handler.handleStep(restricted);
403 handler.finish(restricted.getCurrentState());
404 }
405
406 }
407
408 if (isLastStep) {
409
410 return eventState;
411 }
412
413 if (action == Action.RESET_DERIVATIVES || action == Action.RESET_STATE) {
414
415
416 final FieldODEState<T> newState = occurrence.getNewState();
417 final T[] y = newState.getCompleteState();
418 final T[] yDot = computeDerivatives(newState.getTime(), y);
419 resetOccurred = true;
420 return equations.getMapper().mapStateAndDerivative(newState.getTime(), y, yDot);
421 }
422
423
424
425 previousState = eventState;
426 restricted = restricted.restrictStep(eventState, currentState);
427
428 if (action == Action.RESET_EVENTS) {
429 continue resetEvents;
430 }
431
432
433
434 if (currentEvent.evaluateStep(restricted)) {
435
436 occurringEvents.add(currentEvent);
437 }
438
439 }
440
441
442
443
444
445
446
447
448 for (final FieldDetectorBasedEventState<T> state : detectorBasedEventsStates) {
449 if (state.tryAdvance(currentState, interpolator)) {
450 occurringEvents.add(state);
451 }
452 }
453
454 } while (!occurringEvents.isEmpty());
455
456 doneWithStep = true;
457 } while (!doneWithStep);
458
459
460 isLastStep = isLastStep || currentState.getTime().subtract(tEnd).norm() < FastMath.ulp(tEnd.getReal());
461
462
463 for (FieldODEStepHandler<T> handler : stepHandlers) {
464 handler.handleStep(restricted);
465 if (isLastStep) {
466 handler.finish(restricted.getCurrentState());
467 }
468 }
469
470 return currentState;
471
472 }
473
474
475
476
477
478
479
480
481 protected void sanityChecks(final FieldODEState<T> initialState, final T t)
482 throws MathIllegalArgumentException {
483
484 final double threshold = 1000 * FastMath.ulp(FastMath.max(FastMath.abs(initialState.getTime().getReal()),
485 FastMath.abs(t.getReal())));
486 final double dt = initialState.getTime().subtract(t).norm();
487 if (dt < threshold) {
488 throw new MathIllegalArgumentException(LocalizedODEFormats.TOO_SMALL_INTEGRATION_INTERVAL,
489 dt, threshold, false);
490 }
491
492 }
493
494
495
496
497 protected boolean resetOccurred() {
498 return resetOccurred;
499 }
500
501
502
503
504 protected void setStepSize(final T stepSize) {
505 this.stepSize = stepSize;
506 }
507
508
509
510
511 protected T getStepSize() {
512 return stepSize;
513 }
514
515
516
517
518 protected void setStepStart(final FieldODEStateAndDerivative<T> stepStart) {
519 this.stepStart = stepStart;
520 }
521
522
523 @Override
524 public FieldODEStateAndDerivative<T> getStepStart() {
525 return stepStart;
526 }
527
528
529
530
531 protected void setIsLastStep(final boolean isLastStep) {
532 this.isLastStep = isLastStep;
533 }
534
535
536
537
538 protected boolean isLastStep() {
539 return isLastStep;
540 }
541
542 }