FieldDenseOutputModel.java

  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.  */

  18. /*
  19.  * This is not the original file distributed by the Apache Software Foundation
  20.  * It has been modified by the Hipparchus project
  21.  */

  23. package org.hipparchus.ode;

  25. import java.util.ArrayList;
  26. import java.util.List;

  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;

  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.  */

  84. public class FieldDenseOutputModel<T extends CalculusFieldElement<T>>
  85.     implements FieldODEStepHandler<T> {

  87.     /** Initial integration time. */
  88.     private T initialTime;

  90.     /** Final integration time. */
  91.     private T finalTime;

  93.     /** Integration direction indicator. */
  94.     private boolean forward;

  96.     /** Current interpolator index. */
  97.     private int index;

  99.     /** Steps table. */
  100.     private List<FieldODEStateInterpolator<T>> steps;

  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.     }

  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 {

  126.         if (model.steps.isEmpty()) {
  127.             return;
  128.         }

  130.         if (steps.isEmpty()) {
  131.             initialTime = model.initialTime;
  132.             forward     = model.forward;
  133.         } else {

  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.             }

  144.             if (forward ^ model.forward) {
  145.                 throw new MathIllegalArgumentException(LocalizedODEFormats.PROPAGATION_DIRECTION_MISMATCH);
  146.             }

  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.             }

  158.         }

  160.         steps.addAll(model.steps);

  162.         index = steps.size() - 1;
  163.         finalTime = (steps.get(index)).getCurrentState().getTime();

  165.     }

  167.     /** Check dimensions equality.
  168.      * @param d1 first dimension
  169.      * @param d2 second dimansion
  170.      * @exception MathIllegalArgumentException if dimensions do not match
  171.      */
  172.     private void checkDimensionsEquality(final int d1, final int d2)
  173.         throws MathIllegalArgumentException {
  174.         if (d1 != d2) {
  175.             throw new MathIllegalArgumentException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
  176.                                                    d2, d1);
  177.         }
  178.     }

  180.     /** {@inheritDoc} */
  181.     @Override
  182.     public void init(final FieldODEStateAndDerivative<T> initialState, final T t) {
  183.         initialTime = initialState.getTime();
  184.         finalTime   = t;
  185.         forward     = true;
  186.         index       = 0;
  187.         steps.clear();
  188.     }

  190.     /** {@inheritDoc} */
  191.     @Override
  192.     public void handleStep(final FieldODEStateInterpolator<T> interpolator) {

  194.         if (steps.isEmpty()) {
  195.             initialTime = interpolator.getPreviousState().getTime();
  196.             forward     = interpolator.isForward();
  197.         }

  199.         steps.add(interpolator);
  200.     }

  202.     /** {@inheritDoc} */
  203.     @Override
  204.     public void finish(FieldODEStateAndDerivative<T> finalState) {
  205.         finalTime = finalState.getTime();
  206.         index     = steps.size() - 1;
  207.     }

  209.     /**
  210.      * Get the initial integration time.
  211.      * @return initial integration time
  212.      */
  213.     public T getInitialTime() {
  214.         return initialTime;
  215.     }

  217.     /**
  218.      * Get the final integration time.
  219.      * @return final integration time
  220.      */
  221.     public T getFinalTime() {
  222.         return finalTime;
  223.     }

  225.     /**
  226.      * Get the state at interpolated time.
  227.      * @param time time of the interpolated point
  228.      * @return state at interpolated time
  229.      */
  230.     public FieldODEStateAndDerivative<T> getInterpolatedState(final T time) {

  232.         // initialize the search with the complete steps table
  233.         int iMin = 0;
  234.         final FieldODEStateInterpolator<T> sMin = steps.get(iMin);
  235.         T tMin = sMin.getPreviousState().getTime().add(sMin.getCurrentState().getTime()).multiply(0.5);

  237.         int iMax = steps.size() - 1;
  238.         final FieldODEStateInterpolator<T> sMax = steps.get(iMax);
  239.         T tMax = sMax.getPreviousState().getTime().add(sMax.getCurrentState().getTime()).multiply(0.5);

  241.         // handle points outside of the integration interval
  242.         // or in the first and last step
  243.         if (locatePoint(time, sMin) <= 0) {
  244.             index = iMin;
  245.             return sMin.getInterpolatedState(time);
  246.         }
  247.         if (locatePoint(time, sMax) >= 0) {
  248.             index = iMax;
  249.             return sMax.getInterpolatedState(time);
  250.         }

  252.         // reduction of the table slice size
  253.         while (iMax - iMin > 5) {

  255.             // use the last estimated index as the splitting index
  256.             final FieldODEStateInterpolator<T> si = steps.get(index);
  257.             final int location = locatePoint(time, si);
  258.             if (location < 0) {
  259.                 iMax = index;
  260.                 tMax = si.getPreviousState().getTime().add(si.getCurrentState().getTime()).multiply(0.5);
  261.             } else if (location > 0) {
  262.                 iMin = index;
  263.                 tMin = si.getPreviousState().getTime().add(si.getCurrentState().getTime()).multiply(0.5);
  264.             } else {
  265.                 // we have found the target step, no need to continue searching
  266.                 return si.getInterpolatedState(time);
  267.             }

  269.             // compute a new estimate of the index in the reduced table slice
  270.             final int iMed = (iMin + iMax) / 2;
  271.             final FieldODEStateInterpolator<T> sMed = steps.get(iMed);
  272.             final T tMed = sMed.getPreviousState().getTime().add(sMed.getCurrentState().getTime()).multiply(0.5);

  274.             if (tMed.subtract(tMin).abs().subtract(1.0e-6).getReal() < 0 ||
  275.                 tMax.subtract(tMed).abs().subtract(1.0e-6).getReal() < 0) {
  276.                 // too close to the bounds, we estimate using a simple dichotomy
  277.                 index = iMed;
  278.             } else {
  279.                 // estimate the index using a reverse quadratic polynomial
  280.                 // (reverse means we have i = P(t), thus allowing to simply
  281.                 // compute index = P(time) rather than solving a quadratic equation)
  282.                 final T d12 = tMax.subtract(tMed);
  283.                 final T d23 = tMed.subtract(tMin);
  284.                 final T d13 = tMax.subtract(tMin);
  285.                 final T dt1 = time.subtract(tMax);
  286.                 final T dt2 = time.subtract(tMed);
  287.                 final T dt3 = time.subtract(tMin);
  288.                 final T iLagrange =           dt2.multiply(dt3).multiply(d23).multiply(iMax).
  289.                                      subtract(dt1.multiply(dt3).multiply(d13).multiply(iMed)).
  290.                                      add(     dt1.multiply(dt2).multiply(d12).multiply(iMin)).
  291.                                      divide(d12.multiply(d23).multiply(d13));
  292.                 index = (int) FastMath.rint(iLagrange.getReal());
  293.             }

  295.             // force the next size reduction to be at least one tenth
  296.             final int low  = FastMath.max(iMin + 1, (9 * iMin + iMax) / 10);
  297.             final int high = FastMath.min(iMax - 1, (iMin + 9 * iMax) / 10);
  298.             if (index < low) {
  299.                 index = low;
  300.             } else if (index > high) {
  301.                 index = high;
  302.             }

  304.         }

  306.         // now the table slice is very small, we perform an iterative search
  307.         index = iMin;
  308.         while (index <= iMax && locatePoint(time, steps.get(index)) > 0) {
  309.             ++index;
  310.         }

  312.         return steps.get(index).getInterpolatedState(time);

  314.     }

  316.     /** Compare a step interval and a double.
  317.      * @param time point to locate
  318.      * @param interval step interval
  319.      * @return -1 if the double is before the interval, 0 if it is in
  320.      * the interval, and +1 if it is after the interval, according to
  321.      * the interval direction
  322.      */
  323.     private int locatePoint(final T time, final FieldODEStateInterpolator<T> interval) {
  324.         if (forward) {
  325.             if (time.subtract(interval.getPreviousState().getTime()).getReal() < 0) {
  326.                 return -1;
  327.             } else if (time.subtract(interval.getCurrentState().getTime()).getReal() > 0) {
  328.                 return +1;
  329.             } else {
  330.                 return 0;
  331.             }
  332.         }
  333.         if (time.subtract(interval.getPreviousState().getTime()).getReal() > 0) {
  334.             return -1;
  335.         } else if (time.subtract(interval.getCurrentState().getTime()).getReal() < 0) {
  336.             return +1;
  337.         } else {
  338.             return 0;
  339.         }
  340.     }

  342. }
Created with JaCoCo 0.8.12.202403310830