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.         for (FieldODEStateInterpolator<T> interpolator : model.steps) {
  161.             steps.add(interpolator);
  162.         }

  164.         index = steps.size() - 1;
  165.         finalTime = (steps.get(index)).getCurrentState().getTime();

  167.     }

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

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

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

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

  201.         steps.add(interpolator);
  202.     }

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

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

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

  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) {

  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);

  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);

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

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

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

  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);

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

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

  306.         }

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

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

  316.     }

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

  344. }
Created with JaCoCo 0.8.11.202310140853