| /* |
| * Licensed to the Apache Software Foundation (ASF) under one or more |
| * contributor license agreements. See the NOTICE file distributed with |
| * this work for additional information regarding copyright ownership. |
| * The ASF licenses this file to You under the Apache License, Version 2.0 |
| * (the "License"); you may not use this file except in compliance with |
| * the License. You may obtain a copy of the License at |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, software |
| * distributed under the License is distributed on an "AS IS" BASIS, |
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| * See the License for the specific language governing permissions and |
| * limitations under the License. |
| */ |
| |
| package org.apache.commons.math4.legacy.ode; |
| |
| import java.util.ArrayList; |
| import java.util.List; |
| |
| import org.apache.commons.math4.legacy.core.RealFieldElement; |
| import org.apache.commons.math4.legacy.exception.DimensionMismatchException; |
| import org.apache.commons.math4.legacy.exception.MathIllegalArgumentException; |
| import org.apache.commons.math4.legacy.exception.MaxCountExceededException; |
| import org.apache.commons.math4.legacy.exception.util.LocalizedFormats; |
| import org.apache.commons.math4.legacy.ode.sampling.FieldStepHandler; |
| import org.apache.commons.math4.legacy.ode.sampling.FieldStepInterpolator; |
| import org.apache.commons.math4.core.jdkmath.JdkMath; |
| |
| /** |
| * This class stores all information provided by an ODE integrator |
| * during the integration process and build a continuous model of the |
| * solution from this. |
| * |
| * <p>This class act as a step handler from the integrator point of |
| * view. It is called iteratively during the integration process and |
| * stores a copy of all steps information in a sorted collection for |
| * later use. Once the integration process is over, the user can use |
| * the {@link #getInterpolatedState(RealFieldElement) getInterpolatedState} |
| * method to retrieve this information at any time. It is important to wait |
| * for the integration to be over before attempting to call {@link |
| * #getInterpolatedState(RealFieldElement)} because some internal |
| * variables are set only once the last step has been handled.</p> |
| * |
| * <p>This is useful for example if the main loop of the user |
| * application should remain independent from the integration process |
| * or if one needs to mimic the behaviour of an analytical model |
| * despite a numerical model is used (i.e. one needs the ability to |
| * get the model value at any time or to navigate through the |
| * data).</p> |
| * |
| * <p>If problem modeling is done with several separate |
| * integration phases for contiguous intervals, the same |
| * ContinuousOutputModel can be used as step handler for all |
| * integration phases as long as they are performed in order and in |
| * the same direction. As an example, one can extrapolate the |
| * trajectory of a satellite with one model (i.e. one set of |
| * differential equations) up to the beginning of a maneuver, use |
| * another more complex model including thrusters modeling and |
| * accurate attitude control during the maneuver, and revert to the |
| * first model after the end of the maneuver. If the same continuous |
| * output model handles the steps of all integration phases, the user |
| * do not need to bother when the maneuver begins or ends, he has all |
| * the data available in a transparent manner.</p> |
| * |
| * <p>One should be aware that the amount of data stored in a |
| * ContinuousOutputFieldModel instance can be important if the state vector |
| * is large, if the integration interval is long or if the steps are |
| * small (which can result from small tolerance settings in {@link |
| * org.apache.commons.math4.legacy.ode.nonstiff.AdaptiveStepsizeFieldIntegrator adaptive |
| * step size integrators}).</p> |
| * |
| * @see FieldStepHandler |
| * @see FieldStepInterpolator |
| * @param <T> the type of the field elements |
| * @since 3.6 |
| */ |
| |
| public class ContinuousOutputFieldModel<T extends RealFieldElement<T>> |
| implements FieldStepHandler<T> { |
| |
| /** Initial integration time. */ |
| private T initialTime; |
| |
| /** Final integration time. */ |
| private T finalTime; |
| |
| /** Integration direction indicator. */ |
| private boolean forward; |
| |
| /** Current interpolator index. */ |
| private int index; |
| |
| /** Steps table. */ |
| private List<FieldStepInterpolator<T>> steps; |
| |
| /** Simple constructor. |
| * Build an empty continuous output model. |
| */ |
| public ContinuousOutputFieldModel() { |
| steps = new ArrayList<>(); |
| initialTime = null; |
| finalTime = null; |
| forward = true; |
| index = 0; |
| } |
| |
| /** Append another model at the end of the instance. |
| * @param model model to add at the end of the instance |
| * @exception MathIllegalArgumentException if the model to append is not |
| * compatible with the instance (dimension of the state vector, |
| * propagation direction, hole between the dates) |
| * @exception DimensionMismatchException if the dimensions of the states or |
| * the number of secondary states do not match |
| * @exception MaxCountExceededException if the number of functions evaluations is exceeded |
| * during step finalization |
| */ |
| public void append(final ContinuousOutputFieldModel<T> model) |
| throws MathIllegalArgumentException, MaxCountExceededException { |
| |
| if (model.steps.isEmpty()) { |
| return; |
| } |
| |
| if (steps.isEmpty()) { |
| initialTime = model.initialTime; |
| forward = model.forward; |
| } else { |
| |
| // safety checks |
| final FieldODEStateAndDerivative<T> s1 = steps.get(0).getPreviousState(); |
| final FieldODEStateAndDerivative<T> s2 = model.steps.get(0).getPreviousState(); |
| checkDimensionsEquality(s1.getStateDimension(), s2.getStateDimension()); |
| checkDimensionsEquality(s1.getNumberOfSecondaryStates(), s2.getNumberOfSecondaryStates()); |
| for (int i = 0; i < s1.getNumberOfSecondaryStates(); ++i) { |
| checkDimensionsEquality(s1.getSecondaryStateDimension(i), s2.getSecondaryStateDimension(i)); |
| } |
| |
| if (forward ^ model.forward) { |
| throw new MathIllegalArgumentException(LocalizedFormats.PROPAGATION_DIRECTION_MISMATCH); |
| } |
| |
| final FieldStepInterpolator<T> lastInterpolator = steps.get(index); |
| final T current = lastInterpolator.getCurrentState().getTime(); |
| final T previous = lastInterpolator.getPreviousState().getTime(); |
| final T step = current.subtract(previous); |
| final T gap = model.getInitialTime().subtract(current); |
| if (gap.abs().subtract(step.abs().multiply(1.0e-3)).getReal() > 0) { |
| throw new MathIllegalArgumentException(LocalizedFormats.HOLE_BETWEEN_MODELS_TIME_RANGES, |
| gap.abs().getReal()); |
| } |
| |
| } |
| |
| steps.addAll(model.steps); |
| |
| index = steps.size() - 1; |
| finalTime = (steps.get(index)).getCurrentState().getTime(); |
| |
| } |
| |
| /** Check dimensions equality. |
| * @param d1 first dimension |
| * @param d2 second dimension |
| * @exception DimensionMismatchException if dimensions do not match |
| */ |
| private void checkDimensionsEquality(final int d1, final int d2) |
| throws DimensionMismatchException { |
| if (d1 != d2) { |
| throw new DimensionMismatchException(d2, d1); |
| } |
| } |
| |
| /** {@inheritDoc} */ |
| @Override |
| public void init(final FieldODEStateAndDerivative<T> initialState, final T t) { |
| initialTime = initialState.getTime(); |
| finalTime = t; |
| forward = true; |
| index = 0; |
| steps.clear(); |
| } |
| |
| /** Handle the last accepted step. |
| * A copy of the information provided by the last step is stored in |
| * the instance for later use. |
| * @param interpolator interpolator for the last accepted step. |
| * @param isLast true if the step is the last one |
| * @exception MaxCountExceededException if the number of functions evaluations is exceeded |
| * during step finalization |
| */ |
| @Override |
| public void handleStep(final FieldStepInterpolator<T> interpolator, final boolean isLast) |
| throws MaxCountExceededException { |
| |
| if (steps.isEmpty()) { |
| initialTime = interpolator.getPreviousState().getTime(); |
| forward = interpolator.isForward(); |
| } |
| |
| steps.add(interpolator); |
| |
| if (isLast) { |
| finalTime = interpolator.getCurrentState().getTime(); |
| index = steps.size() - 1; |
| } |
| |
| } |
| |
| /** |
| * Get the initial integration time. |
| * @return initial integration time |
| */ |
| public T getInitialTime() { |
| return initialTime; |
| } |
| |
| /** |
| * Get the final integration time. |
| * @return final integration time |
| */ |
| public T getFinalTime() { |
| return finalTime; |
| } |
| |
| /** |
| * Get the state at interpolated time. |
| * @param time time of the interpolated point |
| * @return state at interpolated time |
| */ |
| public FieldODEStateAndDerivative<T> getInterpolatedState(final T time) { |
| |
| // initialize the search with the complete steps table |
| int iMin = 0; |
| final FieldStepInterpolator<T> sMin = steps.get(iMin); |
| T tMin = sMin.getPreviousState().getTime().add(sMin.getCurrentState().getTime()).multiply(0.5); |
| |
| int iMax = steps.size() - 1; |
| final FieldStepInterpolator<T> sMax = steps.get(iMax); |
| T tMax = sMax.getPreviousState().getTime().add(sMax.getCurrentState().getTime()).multiply(0.5); |
| |
| // handle points outside of the integration interval |
| // or in the first and last step |
| if (locatePoint(time, sMin) <= 0) { |
| index = iMin; |
| return sMin.getInterpolatedState(time); |
| } |
| if (locatePoint(time, sMax) >= 0) { |
| index = iMax; |
| return sMax.getInterpolatedState(time); |
| } |
| |
| // reduction of the table slice size |
| while (iMax - iMin > 5) { |
| |
| // use the last estimated index as the splitting index |
| final FieldStepInterpolator<T> si = steps.get(index); |
| final int location = locatePoint(time, si); |
| if (location < 0) { |
| iMax = index; |
| tMax = si.getPreviousState().getTime().add(si.getCurrentState().getTime()).multiply(0.5); |
| } else if (location > 0) { |
| iMin = index; |
| tMin = si.getPreviousState().getTime().add(si.getCurrentState().getTime()).multiply(0.5); |
| } else { |
| // we have found the target step, no need to continue searching |
| return si.getInterpolatedState(time); |
| } |
| |
| // compute a new estimate of the index in the reduced table slice |
| final int iMed = (iMin + iMax) / 2; |
| final FieldStepInterpolator<T> sMed = steps.get(iMed); |
| final T tMed = sMed.getPreviousState().getTime().add(sMed.getCurrentState().getTime()).multiply(0.5); |
| |
| if (tMed.subtract(tMin).abs().subtract(1.0e-6).getReal() < 0 || |
| tMax.subtract(tMed).abs().subtract(1.0e-6).getReal() < 0) { |
| // too close to the bounds, we estimate using a simple dichotomy |
| index = iMed; |
| } else { |
| // estimate the index using a reverse quadratic polynomial |
| // (reverse means we have i = P(t), thus allowing to simply |
| // compute index = P(time) rather than solving a quadratic equation) |
| final T d12 = tMax.subtract(tMed); |
| final T d23 = tMed.subtract(tMin); |
| final T d13 = tMax.subtract(tMin); |
| final T dt1 = time.subtract(tMax); |
| final T dt2 = time.subtract(tMed); |
| final T dt3 = time.subtract(tMin); |
| final T iLagrange = dt2.multiply(dt3).multiply(d23).multiply(iMax). |
| subtract(dt1.multiply(dt3).multiply(d13).multiply(iMed)). |
| add( dt1.multiply(dt2).multiply(d12).multiply(iMin)). |
| divide(d12.multiply(d23).multiply(d13)); |
| index = (int) JdkMath.rint(iLagrange.getReal()); |
| } |
| |
| // force the next size reduction to be at least one tenth |
| final int low = JdkMath.max(iMin + 1, (9 * iMin + iMax) / 10); |
| final int high = JdkMath.min(iMax - 1, (iMin + 9 * iMax) / 10); |
| if (index < low) { |
| index = low; |
| } else if (index > high) { |
| index = high; |
| } |
| |
| } |
| |
| // now the table slice is very small, we perform an iterative search |
| index = iMin; |
| while (index <= iMax && locatePoint(time, steps.get(index)) > 0) { |
| ++index; |
| } |
| |
| return steps.get(index).getInterpolatedState(time); |
| |
| } |
| |
| /** Compare a step interval and a double. |
| * @param time point to locate |
| * @param interval step interval |
| * @return -1 if the double is before the interval, 0 if it is in |
| * the interval, and +1 if it is after the interval, according to |
| * the interval direction |
| */ |
| private int locatePoint(final T time, final FieldStepInterpolator<T> interval) { |
| if (forward) { |
| if (time.subtract(interval.getPreviousState().getTime()).getReal() < 0) { |
| return -1; |
| } else if (time.subtract(interval.getCurrentState().getTime()).getReal() > 0) { |
| return +1; |
| } else { |
| return 0; |
| } |
| } |
| if (time.subtract(interval.getPreviousState().getTime()).getReal() > 0) { |
| return -1; |
| } else if (time.subtract(interval.getCurrentState().getTime()).getReal() < 0) { |
| return +1; |
| } else { |
| return 0; |
| } |
| } |
| |
| } |
