source: src/main/java/agents/anac/y2019/harddealer/math3/ode/ContinuousOutputFieldModel.java

Last change on this file was 204, checked in by Katsuhide Fujita, 5 years ago

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