1 /*
2 * Licensed to the Apache Software Foundation (ASF) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * The ASF licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * https://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17
18 /*
19 * This is not the original file distributed by the Apache Software Foundation
20 * It has been modified by the Hipparchus project
21 */
22
23 package org.hipparchus.ode;
24
25 import java.util.ArrayList;
26 import java.util.List;
27
28 import org.hipparchus.CalculusFieldElement;
29 import org.hipparchus.exception.LocalizedCoreFormats;
30 import org.hipparchus.exception.MathIllegalArgumentException;
31 import org.hipparchus.exception.MathIllegalStateException;
32 import org.hipparchus.ode.sampling.FieldODEStepHandler;
33 import org.hipparchus.ode.sampling.FieldODEStateInterpolator;
34 import org.hipparchus.util.FastMath;
35
36 /**
37 * This class stores all information provided by an ODE integrator
38 * during the integration process and build a continuous model of the
39 * solution from this.
40 *
41 * <p>This class act as a step handler from the integrator point of
42 * view. It is called iteratively during the integration process and
43 * stores a copy of all steps information in a sorted collection for
44 * later use. Once the integration process is over, the user can use
45 * the {@link #getInterpolatedState(CalculusFieldElement) getInterpolatedState}
46 * method to retrieve this information at any time. It is important to wait
47 * for the integration to be over before attempting to call {@link
48 * #getInterpolatedState(CalculusFieldElement)} because some internal
49 * variables are set only once the last step has been handled.</p>
50 *
51 * <p>This is useful for example if the main loop of the user
52 * application should remain independent from the integration process
53 * or if one needs to mimic the behaviour of an analytical model
54 * despite a numerical model is used (i.e. one needs the ability to
55 * get the model value at any time or to navigate through the
56 * data).</p>
57 *
58 * <p>If problem modeling is done with several separate
59 * integration phases for contiguous intervals, the same
60 * FieldDenseOutputModel can be used as step handler for all
61 * integration phases as long as they are performed in order and in
62 * the same direction. As an example, one can extrapolate the
63 * trajectory of a satellite with one model (i.e. one set of
64 * differential equations) up to the beginning of a maneuver, use
65 * another more complex model including thrusters modeling and
66 * accurate attitude control during the maneuver, and revert to the
67 * first model after the end of the maneuver. If the same continuous
68 * output model handles the steps of all integration phases, the user
69 * do not need to bother when the maneuver begins or ends, he has all
70 * the data available in a transparent manner.</p>
71 *
72 * <p>One should be aware that the amount of data stored in a
73 * FieldDenseOutputModel instance can be important if the state vector
74 * is large, if the integration interval is long or if the steps are
75 * small (which can result from small tolerance settings in {@link
76 * org.hipparchus.ode.nonstiff.AdaptiveStepsizeFieldIntegrator adaptive
77 * step size integrators}).</p>
78 *
79 * @see FieldODEStepHandler
80 * @see FieldODEStateInterpolator
81 * @param <T> the type of the field elements
82 */
83
84 public class FieldDenseOutputModel<T extends CalculusFieldElement<T>>
85 implements FieldODEStepHandler<T> {
86
87 /** Initial integration time. */
88 private T initialTime;
89
90 /** Final integration time. */
91 private T finalTime;
92
93 /** Integration direction indicator. */
94 private boolean forward;
95
96 /** Current interpolator index. */
97 private int index;
98
99 /** Steps table. */
100 private List<FieldODEStateInterpolator<T>> steps;
101
102 /** Simple constructor.
103 * Build an empty continuous output model.
104 */
105 public FieldDenseOutputModel() {
106 steps = new ArrayList<>();
107 initialTime = null;
108 finalTime = null;
109 forward = true;
110 index = 0;
111 }
112
113 /** Append another model at the end of the instance.
114 * @param model model to add at the end of the instance
115 * @exception MathIllegalArgumentException if the model to append is not
116 * compatible with the instance (dimension of the state vector,
117 * propagation direction, hole between the dates)
118 * @exception MathIllegalArgumentException if the dimensions of the states or
119 * the number of secondary states do not match
120 * @exception MathIllegalStateException if the number of functions evaluations is exceeded
121 * during step finalization
122 */
123 public void append(final FieldDenseOutputModel<T> model)
124 throws MathIllegalArgumentException, MathIllegalStateException {
125
126 if (model.steps.isEmpty()) {
127 return;
128 }
129
130 if (steps.isEmpty()) {
131 initialTime = model.initialTime;
132 forward = model.forward;
133 } else {
134
135 // safety checks
136 final FieldODEStateAndDerivative<T> s1 = steps.get(0).getPreviousState();
137 final FieldODEStateAndDerivative<T> s2 = model.steps.get(0).getPreviousState();
138 checkDimensionsEquality(s1.getPrimaryStateDimension(), s2.getPrimaryStateDimension());
139 checkDimensionsEquality(s1.getNumberOfSecondaryStates(), s2.getNumberOfSecondaryStates());
140 for (int i = 0; i < s1.getNumberOfSecondaryStates(); ++i) {
141 checkDimensionsEquality(s1.getSecondaryStateDimension(i), s2.getSecondaryStateDimension(i));
142 }
143
144 if (forward ^ model.forward) {
145 throw new MathIllegalArgumentException(LocalizedODEFormats.PROPAGATION_DIRECTION_MISMATCH);
146 }
147
148 final FieldODEStateInterpolator<T> lastInterpolator = steps.get(index);
149 final T current = lastInterpolator.getCurrentState().getTime();
150 final T previous = lastInterpolator.getPreviousState().getTime();
151 final T step = current.subtract(previous);
152 final T gap = model.getInitialTime().subtract(current);
153 if (gap.abs().subtract(step.abs().multiply(1.0e-3)).getReal() > 0) {
154 throw new MathIllegalArgumentException(LocalizedODEFormats.HOLE_BETWEEN_MODELS_TIME_RANGES,
155 gap.norm());
156 }
157
158 }
159
160 for (FieldODEStateInterpolator<T> interpolator : model.steps) {
161 steps.add(interpolator);
162 }
163
164 index = steps.size() - 1;
165 finalTime = (steps.get(index)).getCurrentState().getTime();
166
167 }
168
169 /** Check dimensions equality.
170 * @param d1 first dimension
171 * @param d2 second dimansion
172 * @exception MathIllegalArgumentException if dimensions do not match
173 */
174 private void checkDimensionsEquality(final int d1, final int d2)
175 throws MathIllegalArgumentException {
176 if (d1 != d2) {
177 throw new MathIllegalArgumentException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
178 d2, d1);
179 }
180 }
181
182 /** {@inheritDoc} */
183 @Override
184 public void init(final FieldODEStateAndDerivative<T> initialState, final T t) {
185 initialTime = initialState.getTime();
186 finalTime = t;
187 forward = true;
188 index = 0;
189 steps.clear();
190 }
191
192 /** {@inheritDoc} */
193 @Override
194 public void handleStep(final FieldODEStateInterpolator<T> interpolator) {
195
196 if (steps.isEmpty()) {
197 initialTime = interpolator.getPreviousState().getTime();
198 forward = interpolator.isForward();
199 }
200
201 steps.add(interpolator);
202 }
203
204 /** {@inheritDoc} */
205 @Override
206 public void finish(FieldODEStateAndDerivative<T> finalState) {
207 finalTime = finalState.getTime();
208 index = steps.size() - 1;
209 }
210
211 /**
212 * Get the initial integration time.
213 * @return initial integration time
214 */
215 public T getInitialTime() {
216 return initialTime;
217 }
218
219 /**
220 * Get the final integration time.
221 * @return final integration time
222 */
223 public T getFinalTime() {
224 return finalTime;
225 }
226
227 /**
228 * Get the state at interpolated time.
229 * @param time time of the interpolated point
230 * @return state at interpolated time
231 */
232 public FieldODEStateAndDerivative<T> getInterpolatedState(final T time) {
233
234 // initialize the search with the complete steps table
235 int iMin = 0;
236 final FieldODEStateInterpolator<T> sMin = steps.get(iMin);
237 T tMin = sMin.getPreviousState().getTime().add(sMin.getCurrentState().getTime()).multiply(0.5);
238
239 int iMax = steps.size() - 1;
240 final FieldODEStateInterpolator<T> sMax = steps.get(iMax);
241 T tMax = sMax.getPreviousState().getTime().add(sMax.getCurrentState().getTime()).multiply(0.5);
242
243 // handle points outside of the integration interval
244 // or in the first and last step
245 if (locatePoint(time, sMin) <= 0) {
246 index = iMin;
247 return sMin.getInterpolatedState(time);
248 }
249 if (locatePoint(time, sMax) >= 0) {
250 index = iMax;
251 return sMax.getInterpolatedState(time);
252 }
253
254 // reduction of the table slice size
255 while (iMax - iMin > 5) {
256
257 // use the last estimated index as the splitting index
258 final FieldODEStateInterpolator<T> si = steps.get(index);
259 final int location = locatePoint(time, si);
260 if (location < 0) {
261 iMax = index;
262 tMax = si.getPreviousState().getTime().add(si.getCurrentState().getTime()).multiply(0.5);
263 } else if (location > 0) {
264 iMin = index;
265 tMin = si.getPreviousState().getTime().add(si.getCurrentState().getTime()).multiply(0.5);
266 } else {
267 // we have found the target step, no need to continue searching
268 return si.getInterpolatedState(time);
269 }
270
271 // compute a new estimate of the index in the reduced table slice
272 final int iMed = (iMin + iMax) / 2;
273 final FieldODEStateInterpolator<T> sMed = steps.get(iMed);
274 final T tMed = sMed.getPreviousState().getTime().add(sMed.getCurrentState().getTime()).multiply(0.5);
275
276 if (tMed.subtract(tMin).abs().subtract(1.0e-6).getReal() < 0 ||
277 tMax.subtract(tMed).abs().subtract(1.0e-6).getReal() < 0) {
278 // too close to the bounds, we estimate using a simple dichotomy
279 index = iMed;
280 } else {
281 // estimate the index using a reverse quadratic polynomial
282 // (reverse means we have i = P(t), thus allowing to simply
283 // compute index = P(time) rather than solving a quadratic equation)
284 final T d12 = tMax.subtract(tMed);
285 final T d23 = tMed.subtract(tMin);
286 final T d13 = tMax.subtract(tMin);
287 final T dt1 = time.subtract(tMax);
288 final T dt2 = time.subtract(tMed);
289 final T dt3 = time.subtract(tMin);
290 final T iLagrange = dt2.multiply(dt3).multiply(d23).multiply(iMax).
291 subtract(dt1.multiply(dt3).multiply(d13).multiply(iMed)).
292 add( dt1.multiply(dt2).multiply(d12).multiply(iMin)).
293 divide(d12.multiply(d23).multiply(d13));
294 index = (int) FastMath.rint(iLagrange.getReal());
295 }
296
297 // force the next size reduction to be at least one tenth
298 final int low = FastMath.max(iMin + 1, (9 * iMin + iMax) / 10);
299 final int high = FastMath.min(iMax - 1, (iMin + 9 * iMax) / 10);
300 if (index < low) {
301 index = low;
302 } else if (index > high) {
303 index = high;
304 }
305
306 }
307
308 // now the table slice is very small, we perform an iterative search
309 index = iMin;
310 while (index <= iMax && locatePoint(time, steps.get(index)) > 0) {
311 ++index;
312 }
313
314 return steps.get(index).getInterpolatedState(time);
315
316 }
317
318 /** Compare a step interval and a double.
319 * @param time point to locate
320 * @param interval step interval
321 * @return -1 if the double is before the interval, 0 if it is in
322 * the interval, and +1 if it is after the interval, according to
323 * the interval direction
324 */
325 private int locatePoint(final T time, final FieldODEStateInterpolator<T> interval) {
326 if (forward) {
327 if (time.subtract(interval.getPreviousState().getTime()).getReal() < 0) {
328 return -1;
329 } else if (time.subtract(interval.getCurrentState().getTime()).getReal() > 0) {
330 return +1;
331 } else {
332 return 0;
333 }
334 }
335 if (time.subtract(interval.getPreviousState().getTime()).getReal() > 0) {
336 return -1;
337 } else if (time.subtract(interval.getCurrentState().getTime()).getReal() < 0) {
338 return +1;
339 } else {
340 return 0;
341 }
342 }
343
344 }