FieldEcksteinHechlerPropagator.java

  1. /* Copyright 2002-2018 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS 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. package org.orekit.propagation.analytical;

  19. import org.hipparchus.Field;
  20. import org.hipparchus.RealFieldElement;
  21. import org.hipparchus.analysis.differentiation.FDSFactory;
  22. import org.hipparchus.analysis.differentiation.FieldDerivativeStructure;
  23. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  24. import org.hipparchus.util.FastMath;
  25. import org.hipparchus.util.MathArrays;
  26. import org.orekit.attitudes.AttitudeProvider;
  27. import org.orekit.errors.OrekitException;
  28. import org.orekit.errors.OrekitMessages;
  29. import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider;
  30. import org.orekit.orbits.FieldCartesianOrbit;
  31. import org.orekit.orbits.FieldCircularOrbit;
  32. import org.orekit.orbits.FieldOrbit;
  33. import org.orekit.orbits.OrbitType;
  34. import org.orekit.orbits.PositionAngle;
  35. import org.orekit.propagation.FieldSpacecraftState;
  36. import org.orekit.time.FieldAbsoluteDate;
  37. import org.orekit.utils.FieldTimeSpanMap;
  38. import org.orekit.utils.TimeStampedFieldPVCoordinates;

  40. /** This class propagates a {@link org.orekit.propagation.FieldSpacecraftState}
  41.  *  using the analytical Eckstein-Hechler model.
  42.  * <p>The Eckstein-Hechler model is suited for near circular orbits
  43.  * (e < 0.1, with poor accuracy between 0.005 and 0.1) and inclination
  44.  * neither equatorial (direct or retrograde) nor critical (direct or
  45.  * retrograde).</p>
  46.  * @see FieldOrbit
  47.  * @author Guylaine Prat
  48.  */
  49. public class FieldEcksteinHechlerPropagator<T extends RealFieldElement<T>> extends FieldAbstractAnalyticalPropagator<T> {

  51.     /** Factory for the derivatives. */
  52.     private final FDSFactory<T> factory;

  54.     /** Initial Eckstein-Hechler model. */
  55.     private FieldEHModel<T> initialModel;

  57.     /** All models. */
  58.     private transient FieldTimeSpanMap<FieldEHModel<T>, T> models;

  60.     /** Reference radius of the central body attraction model (m). */
  61.     private double referenceRadius;

  63.     /** Central attraction coefficient (m³/s²). */
  64.     private double mu;

  66.     /** Un-normalized zonal coefficients. */
  67.     private double[] ck0;

  69.     /** Build a propagator from FieldOrbit<T> and potential provider.
  70.      * <p>Mass and attitude provider are set to unspecified non-null arbitrary values.</p>
  71.      * @param initialOrbit initial FieldOrbit<T>
  72.      * @param provider for un-normalized zonal coefficients
  73.      * @exception OrekitException if the zonal coefficients cannot be retrieved
  74.      * or if the mean parameters cannot be computed
  75.      */
  76.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
  77.                                           final UnnormalizedSphericalHarmonicsProvider provider)
  78.         throws OrekitException {
  79.         this(initialOrbit, DEFAULT_LAW, initialOrbit.getA().getField().getZero().add(DEFAULT_MASS), provider,
  80.              provider.onDate(initialOrbit.getDate().toAbsoluteDate()));
  81.     }

  83.     /**
  84.      * Private helper constructor.
  85.      * @param initialOrbit initial FieldOrbit<T>
  86.      * @param attitude attitude provider
  87.      * @param mass spacecraft mass
  88.      * @param provider for un-normalized zonal coefficients
  89.      * @param harmonics {@code provider.onDate(initialOrbit.getDate())}
  90.      * @exception OrekitException if the zonal coefficients cannot be retrieved
  91.      */
  92.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
  93.                                           final  AttitudeProvider attitude,
  94.                                           final T mass,
  95.                                           final UnnormalizedSphericalHarmonicsProvider provider,
  96.                                           final UnnormalizedSphericalHarmonicsProvider.UnnormalizedSphericalHarmonics harmonics)
  97.         throws OrekitException {
  98.         this(initialOrbit, attitude,  mass, provider.getAe(), provider.getMu(),
  99.              harmonics.getUnnormalizedCnm(2, 0),
  100.              harmonics.getUnnormalizedCnm(3, 0),
  101.              harmonics.getUnnormalizedCnm(4, 0),
  102.              harmonics.getUnnormalizedCnm(5, 0),
  103.              harmonics.getUnnormalizedCnm(6, 0));
  104.     }

  106.     /** Build a propagator from FieldOrbit<T> and potential.
  107.      * <p>Mass and attitude provider are set to unspecified non-null arbitrary values.</p>
  108.      * <p>The C<sub>n,0</sub> coefficients are the denormalized zonal coefficients, they
  109.      * are related to both the normalized coefficients
  110.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  111.      *  and the J<sub>n</sub> one as follows:</p>
  112.      * <pre>
  113.      *   C<sub>n,0</sub> = [(2-δ<sub>0,m</sub>)(2n+1)(n-m)!/(n+m)!]<sup>½</sup>
  114.      *                      <span style="text-decoration: overline">C</span><sub>n,0</sub>
  115.      *   C<sub>n,0</sub> = -J<sub>n</sub>
  116.      * </pre>
  117.      * @param initialOrbit initial FieldOrbit<T>
  118.      * @param referenceRadius reference radius of the Earth for the potential model (m)
  119.      * @param mu central attraction coefficient (m³/s²)
  120.      * @param c20 un-normalized zonal coefficient (about -1.08e-3 for Earth)
  121.      * @param c30 un-normalized zonal coefficient (about +2.53e-6 for Earth)
  122.      * @param c40 un-normalized zonal coefficient (about +1.62e-6 for Earth)
  123.      * @param c50 un-normalized zonal coefficient (about +2.28e-7 for Earth)
  124.      * @param c60 un-normalized zonal coefficient (about -5.41e-7 for Earth)
  125.      * @exception OrekitException if the mean parameters cannot be computed
  126.      * @see org.orekit.utils.Constants
  127.      */
  128.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
  129.                                           final double referenceRadius, final double mu,
  130.                                           final double c20, final double c30, final double c40,
  131.                                           final double c50, final double c60)
  132.         throws OrekitException {
  133.         this(initialOrbit, DEFAULT_LAW, initialOrbit.getDate().getField().getZero().add(DEFAULT_MASS),
  134.              referenceRadius, mu, c20, c30, c40, c50, c60);
  135.     }

  137.     /** Build a propagator from FieldOrbit<T>, mass and potential provider.
  138.      * <p>Attitude law is set to an unspecified non-null arbitrary value.</p>
  139.      * @param initialOrbit initial FieldOrbit<T>
  140.      * @param mass spacecraft mass
  141.      * @param provider for un-normalized zonal coefficients
  142.      * @exception OrekitException if the zonal coefficients cannot be retrieved
  143.      * or if the mean parameters cannot be computed
  144.      */
  145.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit, final T mass,
  146.                                           final UnnormalizedSphericalHarmonicsProvider provider)
  147.         throws OrekitException {
  148.         this(initialOrbit, DEFAULT_LAW, mass, provider, provider.onDate(initialOrbit.getDate().toAbsoluteDate()));
  149.     }

  151.     /** Build a propagator from FieldOrbit<T>, mass and potential.
  152.      * <p>Attitude law is set to an unspecified non-null arbitrary value.</p>
  153.      * <p>The C<sub>n,0</sub> coefficients are the denormalized zonal coefficients, they
  154.      * are related to both the normalized coefficients
  155.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  156.      *  and the J<sub>n</sub> one as follows:</p>
  157.      * <pre>
  158.      *   C<sub>n,0</sub> = [(2-δ<sub>0,m</sub>)(2n+1)(n-m)!/(n+m)!]<sup>½</sup>
  159.      *                      <span style="text-decoration: overline">C</span><sub>n,0</sub>
  160.      *   C<sub>n,0</sub> = -J<sub>n</sub>
  161.      * </pre>
  162.      * @param initialOrbit initial FieldOrbit<T>
  163.      * @param mass spacecraft mass
  164.      * @param referenceRadius reference radius of the Earth for the potential model (m)
  165.      * @param mu central attraction coefficient (m³/s²)
  166.      * @param c20 un-normalized zonal coefficient (about -1.08e-3 for Earth)
  167.      * @param c30 un-normalized zonal coefficient (about +2.53e-6 for Earth)
  168.      * @param c40 un-normalized zonal coefficient (about +1.62e-6 for Earth)
  169.      * @param c50 un-normalized zonal coefficient (about +2.28e-7 for Earth)
  170.      * @param c60 un-normalized zonal coefficient (about -5.41e-7 for Earth)
  171.      * @exception OrekitException if the mean parameters cannot be computed
  172.      */
  173.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit, final T mass,
  174.                                           final double referenceRadius, final double mu,
  175.                                           final double c20, final double c30, final double c40,
  176.                                           final double c50, final double c60)
  177.         throws OrekitException {
  178.         this(initialOrbit, DEFAULT_LAW, mass, referenceRadius, mu, c20, c30, c40, c50, c60);
  179.     }

  181.     /** Build a propagator from FieldOrbit<T>, attitude provider and potential provider.
  182.      * <p>Mass is set to an unspecified non-null arbitrary value.</p>
  183.      * @param initialOrbit initial FieldOrbit<T>
  184.      * @param attitudeProv attitude provider
  185.      * @param provider for un-normalized zonal coefficients
  186.      * @exception OrekitException if the zonal coefficients cannot be retrieved
  187.      * or if the mean parameters cannot be computed
  188.      */
  189.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
  190.                                           final AttitudeProvider attitudeProv,
  191.                                           final UnnormalizedSphericalHarmonicsProvider provider)
  192.         throws OrekitException {
  193.         this(initialOrbit, attitudeProv, initialOrbit.getA().getField().getZero().add(DEFAULT_MASS), provider, provider.onDate(initialOrbit.getDate().toAbsoluteDate()));
  194.     }

  196.     /** Build a propagator from FieldOrbit<T>, attitude provider and potential.
  197.      * <p>Mass is set to an unspecified non-null arbitrary value.</p>
  198.      * <p>The C<sub>n,0</sub> coefficients are the denormalized zonal coefficients, they
  199.      * are related to both the normalized coefficients
  200.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  201.      *  and the J<sub>n</sub> one as follows:</p>
  202.      * <pre>
  203.      *   C<sub>n,0</sub> = [(2-δ<sub>0,m</sub>)(2n+1)(n-m)!/(n+m)!]<sup>½</sup>
  204.      *                     <span style="text-decoration: overline">C</span><sub>n,0</sub>
  205.      *   C<sub>n,0</sub> = -J<sub>n</sub>
  206.      * </pre>
  207.      * @param initialOrbit initial FieldOrbit<T>
  208.      * @param attitudeProv attitude provider
  209.      * @param referenceRadius reference radius of the Earth for the potential model (m)
  210.      * @param mu central attraction coefficient (m³/s²)
  211.      * @param c20 un-normalized zonal coefficient (about -1.08e-3 for Earth)
  212.      * @param c30 un-normalized zonal coefficient (about +2.53e-6 for Earth)
  213.      * @param c40 un-normalized zonal coefficient (about +1.62e-6 for Earth)
  214.      * @param c50 un-normalized zonal coefficient (about +2.28e-7 for Earth)
  215.      * @param c60 un-normalized zonal coefficient (about -5.41e-7 for Earth)
  216.      * @exception OrekitException if the mean parameters cannot be computed
  217.      */
  218.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
  219.                                           final AttitudeProvider attitudeProv,
  220.                                           final double referenceRadius, final double mu,
  221.                                           final double c20, final double c30, final double c40,
  222.                                           final double c50, final double c60)
  223.         throws OrekitException {
  224.         this(initialOrbit, attitudeProv, initialOrbit.getDate().getField().getZero().add(DEFAULT_MASS),
  225.              referenceRadius, mu, c20, c30, c40, c50, c60);
  226.     }

  228.     /** Build a propagator from FieldOrbit<T>, attitude provider, mass and potential provider.
  229.      * @param initialOrbit initial FieldOrbit<T>
  230.      * @param attitudeProv attitude provider
  231.      * @param mass spacecraft mass
  232.      * @param provider for un-normalized zonal coefficients
  233.      * @exception OrekitException if the zonal coefficients cannot be retrieved
  234.      * or if the mean parameters cannot be computed
  235.      */
  236.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
  237.                                           final AttitudeProvider attitudeProv,
  238.                                           final T mass,
  239.                                           final UnnormalizedSphericalHarmonicsProvider provider)
  240.         throws OrekitException {
  241.         this(initialOrbit, attitudeProv, mass, provider, provider.onDate(initialOrbit.getDate().toAbsoluteDate()));
  242.     }

  244.     /** Build a propagator from FieldOrbit<T>, attitude provider, mass and potential.
  245.      * <p>The C<sub>n,0</sub> coefficients are the denormalized zonal coefficients, they
  246.      * are related to both the normalized coefficients
  247.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  248.      *  and the J<sub>n</sub> one as follows:</p>
  249.      * <pre>
  250.      *   C<sub>n,0</sub> = [(2-δ<sub>0,m</sub>)(2n+1)(n-m)!/(n+m)!]<sup>½</sup>
  251.      *                      <span style="text-decoration: overline">C</span><sub>n,0</sub>
  252.      *   C<sub>n,0</sub> = -J<sub>n</sub>
  253.      * </pre>
  254.      * @param initialOrbit initial FieldOrbit<T>
  255.      * @param attitudeProv attitude provider
  256.      * @param mass spacecraft mass
  257.      * @param referenceRadius reference radius of the Earth for the potential model (m)
  258.      * @param mu central attraction coefficient (m³/s²)
  259.      * @param c20 un-normalized zonal coefficient (about -1.08e-3 for Earth)
  260.      * @param c30 un-normalized zonal coefficient (about +2.53e-6 for Earth)
  261.      * @param c40 un-normalized zonal coefficient (about +1.62e-6 for Earth)
  262.      * @param c50 un-normalized zonal coefficient (about +2.28e-7 for Earth)
  263.      * @param c60 un-normalized zonal coefficient (about -5.41e-7 for Earth)
  264.      * @exception OrekitException if the mean parameters cannot be computed
  265.      */
  266.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
  267.                                           final AttitudeProvider attitudeProv,
  268.                                           final T mass,
  269.                                           final double referenceRadius, final double mu,
  270.                                           final double c20, final double c30, final double c40,
  271.                                           final double c50, final double c60)
  272.         throws OrekitException {

  274.         super(mass.getField(), attitudeProv);
  275.         final Field<T> field = mass.getField();
  276.         factory = new FDSFactory<>(field, 1, 2);
  277.         try {

  279.             // store model coefficients
  280.             this.referenceRadius = referenceRadius;
  281.             this.mu  = mu;
  282.             this.ck0 = new double[] {
  283.                 0.0, 0.0, c20, c30, c40, c50, c60
  284.             };

  286.             // compute mean parameters
  287.             // transform into circular adapted parameters used by the Eckstein-Hechler model
  288.             resetInitialState(new FieldSpacecraftState<>(initialOrbit,
  289.                                                          attitudeProv.getAttitude(initialOrbit,
  290.                                                                                   initialOrbit.getDate(),
  291.                                                                                   initialOrbit.getFrame()),
  292.                                                          mass));

  294.         } catch (OrekitException oe) {
  295.             throw new OrekitException(oe);
  296.         }
  297.     }

  299.     /** {@inheritDoc} */
  300.     public void resetInitialState(final FieldSpacecraftState<T> state)
  301.         throws OrekitException {
  302.         super.resetInitialState(state);
  303.         this.initialModel = computeMeanParameters((FieldCircularOrbit<T>) OrbitType.CIRCULAR.convertType(state.getOrbit()),
  304.                                                   state.getMass());
  305.         this.models       = new FieldTimeSpanMap<FieldEHModel<T>, T>(initialModel, state.getA().getField());
  306.     }

  308.     /** {@inheritDoc} */
  309.     protected void resetIntermediateState(final FieldSpacecraftState<T> state, final boolean forward)
  310.         throws OrekitException {
  311.         final FieldEHModel<T> newModel = computeMeanParameters((FieldCircularOrbit<T>) OrbitType.CIRCULAR.convertType(state.getOrbit()),
  312.                                                        state.getMass());
  313.         if (forward) {
  314.             models.addValidAfter(newModel, state.getDate());
  315.         } else {
  316.             models.addValidBefore(newModel, state.getDate());
  317.         }
  318.     }

  320.     /** Compute mean parameters according to the Eckstein-Hechler analytical model.
  321.      * @param osculating osculating FieldOrbit<T>
  322.      * @param mass constant mass
  323.      * @return Eckstein-Hechler mean model
  324.      * @exception OrekitException if FieldOrbit<T> goes outside of supported range
  325.      * (trajectory inside the Brillouin sphere, too eccentric, equatorial, critical
  326.      * inclination) or if convergence cannot be reached
  327.      */
  328.     private FieldEHModel<T> computeMeanParameters(final FieldCircularOrbit<T> osculating, final T mass)
  329.         throws OrekitException {

  331.         // sanity check
  332.         if (osculating.getA().getReal() < referenceRadius) {
  333.             throw new OrekitException(OrekitMessages.TRAJECTORY_INSIDE_BRILLOUIN_SPHERE,
  334.                                            osculating.getA());
  335.         }
  336.         final Field<T> field = mass.getField();
  337.         final T one = field.getOne();
  338.         final T zero = field.getZero();
  339.         // rough initialization of the mean parameters
  340.         FieldEHModel<T> current = new FieldEHModel<>(factory, osculating, mass, referenceRadius, mu, ck0);
  341.         // threshold for each parameter
  342.         final T epsilon         = one .multiply(1.0e-13);
  343.         final T thresholdA      = epsilon.multiply(current.mean.getA().abs().add(1.0));
  344.         final T thresholdE      = epsilon.multiply(current.mean.getE().add(1.0));
  345.         final T thresholdAngles = epsilon.multiply(FastMath.PI);


  348.         int i = 0;
  349.         while (i++ < 100) {

  351.             // recompute the osculating parameters from the current mean parameters
  352.             final FieldDerivativeStructure<T>[] parameters = current.propagateParameters(current.mean.getDate());
  353.             // adapted parameters residuals
  354.             final T deltaA      = osculating.getA()         .subtract(parameters[0].getValue());
  355.             final T deltaEx     = osculating.getCircularEx().subtract(parameters[1].getValue());
  356.             final T deltaEy     = osculating.getCircularEy().subtract(parameters[2].getValue());
  357.             final T deltaI      = osculating.getI()         .subtract(parameters[3].getValue());
  358.             final T deltaRAAN   = normalizeAngle(osculating.getRightAscensionOfAscendingNode().subtract(
  359.                                                                 parameters[4].getValue()),
  360.                                                                 zero);
  361.             final T deltaAlphaM = normalizeAngle(osculating.getAlphaM().subtract(parameters[5].getValue()), zero);
  362.             // update mean parameters
  363.             current = new FieldEHModel<>(factory,
  364.                                          new FieldCircularOrbit<>(current.mean.getA().add(deltaA),
  365.                                                                   current.mean.getCircularEx().add( deltaEx),
  366.                                                                   current.mean.getCircularEy().add( deltaEy),
  367.                                                                   current.mean.getI()         .add( deltaI ),
  368.                                                                   current.mean.getRightAscensionOfAscendingNode().add(deltaRAAN),
  369.                                                                   current.mean.getAlphaM().add(deltaAlphaM),
  370.                                                                   PositionAngle.MEAN,
  371.                                                                   current.mean.getFrame(),
  372.                                                                   current.mean.getDate(), mu),
  373.                                   mass, referenceRadius, mu, ck0);
  374.             // check convergence
  375.             if ((FastMath.abs(deltaA.getReal())      < thresholdA.getReal()) &&
  376.                 (FastMath.abs(deltaEx.getReal())     < thresholdE.getReal()) &&
  377.                 (FastMath.abs(deltaEy.getReal())     < thresholdE.getReal()) &&
  378.                 (FastMath.abs(deltaI.getReal())      < thresholdAngles.getReal()) &&
  379.                 (FastMath.abs(deltaRAAN.getReal())   < thresholdAngles.getReal()) &&
  380.                 (FastMath.abs(deltaAlphaM.getReal()) < thresholdAngles.getReal())) {
  381.                 return current;
  382.             }

  384.         }

  386.         throw new OrekitException(OrekitMessages.UNABLE_TO_COMPUTE_ECKSTEIN_HECHLER_MEAN_PARAMETERS, i);

  388.     }

  390.     /** {@inheritDoc} */
  391.     public FieldCartesianOrbit<T> propagateOrbit(final FieldAbsoluteDate<T> date)
  392.         throws OrekitException {
  393.         // compute Cartesian parameters, taking derivatives into account
  394.         // to make sure velocity and acceleration are consistent
  395.         final FieldEHModel<T> current = models.get(date);
  396.         return new FieldCartesianOrbit<>(toCartesian(date, current.propagateParameters(date)),
  397.                                          current.mean.getFrame(), mu);
  398.     }

  400.     /** Local class for Eckstein-Hechler model, with fixed mean parameters. */
  401.     private static class FieldEHModel<T extends RealFieldElement<T>> {

  403.         /** Factory for the derivatives. */
  404.         private final FDSFactory<T> factory;

  406.         /** Mean FieldOrbit<T>. */
  407.         private final FieldCircularOrbit<T> mean;

  409.         /** Constant mass. */
  410.         private final T mass;
  411.         // CHECKSTYLE: stop JavadocVariable check

  413.         // preprocessed values
  414.         private final T xnotDot;
  415.         private final T rdpom;
  416.         private final T rdpomp;
  417.         private final T eps1;
  418.         private final T eps2;
  419.         private final T xim;
  420.         private final T ommD;
  421.         private final T rdl;
  422.         private final T aMD;

  424.         private final T kh;
  425.         private final T kl;

  427.         private final T ax1;
  428.         private final T ay1;
  429.         private final T as1;
  430.         private final T ac2;
  431.         private final T axy3;
  432.         private final T as3;
  433.         private final T ac4;
  434.         private final T as5;
  435.         private final T ac6;

  437.         private final T ex1;
  438.         private final T exx2;
  439.         private final T exy2;
  440.         private final T ex3;
  441.         private final T ex4;

  443.         private final T ey1;
  444.         private final T eyx2;
  445.         private final T eyy2;
  446.         private final T ey3;
  447.         private final T ey4;

  449.         private final T rx1;
  450.         private final T ry1;
  451.         private final T r2;
  452.         private final T r3;
  453.         private final T rl;

  455.         private final T iy1;
  456.         private final T ix1;
  457.         private final T i2;
  458.         private final T i3;
  459.         private final T ih;

  461.         private final T lx1;
  462.         private final T ly1;
  463.         private final T l2;
  464.         private final T l3;
  465.         private final T ll;

  467.         // CHECKSTYLE: resume JavadocVariable check

  469.         /** Create a model for specified mean FieldOrbit<T>.
  470.          * @param factory factory for the derivatives
  471.          * @param mean mean FieldOrbit<T>
  472.          * @param mass constant mass
  473.          * @param referenceRadius reference radius of the central body attraction model (m)
  474.          * @param mu central attraction coefficient (m³/s²)
  475.          * @param ck0 un-normalized zonal coefficients
  476.          * @exception OrekitException if mean FieldOrbit<T> is not within model supported domain
  477.          */
  478.         FieldEHModel(final FDSFactory<T> factory, final FieldCircularOrbit<T> mean, final T mass,
  479.                      final double referenceRadius, final double mu, final double[] ck0)
  480.             throws OrekitException {

  482.             this.factory         = factory;
  483.             this.mean            = mean;
  484.             this.mass            = mass;
  485.             final T zero = mass.getField().getZero();
  486.             final T one  = mass.getField().getOne();
  487.             // preliminary processing
  488.             T q =  zero.add(referenceRadius).divide(mean.getA());
  489.             T ql = q.multiply(q);
  490.             final T g2 = ql.multiply(ck0[2]);
  491.             ql = ql.multiply(q);
  492.             final T g3 = ql.multiply(ck0[3]);
  493.             ql = ql.multiply(q);
  494.             final T g4 = ql.multiply(ck0[4]);
  495.             ql = ql.multiply(q);
  496.             final T g5 = ql.multiply(ck0[5]);
  497.             ql = ql.multiply(q);
  498.             final T g6 = ql.multiply(ck0[6]);

  500.             final T cosI1 = mean.getI().cos();
  501.             final T sinI1 = mean.getI().sin();
  502.             final T sinI2 = sinI1.multiply(sinI1);
  503.             final T sinI4 = sinI2.multiply(sinI2);
  504.             final T sinI6 = sinI2.multiply(sinI4);

  506.             if (sinI2.getReal() < 1.0e-10) {
  507.                 throw new OrekitException(OrekitMessages.ALMOST_EQUATORIAL_ORBIT,
  508.                                                FastMath.toDegrees(mean.getI().getReal()));
  509.             }

  511.             if (FastMath.abs(sinI2.getReal() - 4.0 / 5.0) < 1.0e-3) {
  512.                 throw new OrekitException(OrekitMessages.ALMOST_CRITICALLY_INCLINED_ORBIT,
  513.                                                FastMath.toDegrees(mean.getI().getReal()));
  514.             }

  516.             if (mean.getE().getReal() > 0.1) {
  517.                 // if 0.005 < e < 0.1 no error is triggered, but accuracy is poor
  518.                 throw new OrekitException(OrekitMessages.TOO_LARGE_ECCENTRICITY_FOR_PROPAGATION_MODEL,
  519.                                                mean.getE());
  520.             }

  522.             xnotDot = zero.add(mu).divide(mean.getA()).sqrt().divide(mean.getA());

  524.             rdpom = g2.multiply(-0.75).multiply(sinI2.multiply(-5.0).add(4.0));
  525.             rdpomp = g4.multiply(7.5).multiply(sinI2.multiply(-31.0 / 8.0).add(1.0).add( sinI4.multiply(49.0 / 16.0))).subtract(
  526.                     g6.multiply(13.125).multiply(one.subtract(sinI2.multiply(8.0)).add(sinI4.multiply(129.0 / 8.0)).subtract(sinI6.multiply(297.0 / 32.0)) ));


  529.             q = zero.add(3.0).divide(rdpom.multiply(32.0));
  530.             eps1 = q.multiply(g4).multiply(sinI2).multiply(sinI2.multiply(-35.0).add(30.0)).subtract(
  531.                    q.multiply(175.0).multiply(g6).multiply(sinI2).multiply(sinI2.multiply(-3.0).add(sinI4.multiply(2.0625)).add(1.0)));
  532.             q = sinI1.multiply(3.0).divide(rdpom.multiply(8.0));
  533.             eps2 = q.multiply(g3).multiply(sinI2.multiply(-5.0).add(4.0)).subtract(q.multiply(g5).multiply(sinI2.multiply(-35.0).add(sinI4.multiply(26.25)).add(10.0)));

  535.             xim = mean.getI();
  536.             ommD = cosI1.multiply(g2.multiply(1.50).subtract(g2.multiply(2.25).multiply(g2).multiply(sinI2.multiply(-19.0 / 6.0).add(2.5))).add(
  537.                             g4.multiply(0.9375).multiply(sinI2.multiply(7.0).subtract(4.0))).add(
  538.                             g6.multiply(3.28125).multiply(sinI2.multiply(-9.0).add(2.0).add(sinI4.multiply(8.25)))));

  540.             rdl = g2.multiply(-1.50).multiply(sinI2.multiply(-4.0).add(3.0)).add(1.0);
  541.             aMD = rdl.add(
  542.                     g2.multiply(2.25).multiply(g2.multiply(sinI2.multiply(-263.0 / 12.0 ).add(9.0).add(sinI4.multiply(341.0 / 24.0))))).add(
  543.                     g4.multiply(15.0 / 16.0).multiply(sinI2.multiply(-31.0).add(8.0).add(sinI4.multiply(24.5)))).add(
  544.                     g6.multiply(105.0 / 32.0).multiply(sinI2.multiply(25.0).add(-10.0 / 3.0).subtract(sinI4.multiply(48.75)).add(sinI6.multiply(27.5))));

  546.             final T qq   = g2.divide(rdl).multiply(-1.5);
  547.             final T qA   = g2.multiply(0.75).multiply(g2).multiply(sinI2);
  548.             final T qB   = g4.multiply(0.25).multiply(sinI2);
  549.             final T qC   = g6.multiply(105.0 / 16.0).multiply(sinI2);
  550.             final T qD   = g3.multiply(-0.75).multiply(sinI1);
  551.             final T qE   = g5.multiply(3.75).multiply(sinI1);
  552.             kh = zero.add(0.375).divide(rdpom);
  553.             kl = kh.divide(sinI1);

  555.             ax1 = qq.multiply(sinI2.multiply(-3.5).add(2.0));
  556.             ay1 = qq.multiply(sinI2.multiply(-2.5).add(2.0));
  557.             as1 = qD.multiply(sinI2.multiply(-5.0).add(4.0)).add(
  558.                   qE.multiply(sinI4.multiply(2.625).add(sinI2.multiply(-3.5)).add(1.0)));
  559.             ac2 = qq.multiply(sinI2).add(
  560.                   qA.multiply(7.0).multiply(sinI2.multiply(-3.0).add(2.0))).add(
  561.                   qB.multiply(sinI2.multiply(-17.5).add(15.0))).add(
  562.                   qC.multiply(sinI2.multiply(3.0).subtract(1.0).subtract(sinI4.multiply(33.0 / 16.0))));
  563.             axy3 = qq.multiply(3.5).multiply(sinI2);
  564.             as3 = qD.multiply(5.0 / 3.0).multiply(sinI2).add(
  565.                   qE.multiply(7.0 / 6.0).multiply(sinI2).multiply(sinI2.multiply(-1.125).add(1)));
  566.             ac4 = qA.multiply(sinI2).add(
  567.                   qB.multiply(4.375).multiply(sinI2)).add(
  568.                   qC.multiply(0.75).multiply(sinI4.multiply(1.1).subtract(sinI2)));

  570.             as5 = qE.multiply(21.0 / 80.0).multiply(sinI4);

  572.             ac6 = qC.multiply(-11.0 / 80.0).multiply(sinI4);

  574.             ex1 = qq.multiply(sinI2.multiply(-1.25).add(1.0));
  575.             exx2 = qq.multiply(0.5).multiply(sinI2.multiply(-5.0).add(3.0));
  576.             exy2 = qq.multiply(sinI2.multiply(-1.5).add(2.0));
  577.             ex3 = qq.multiply(7.0 / 12.0).multiply(sinI2);
  578.             ex4 = qq.multiply(17.0 / 8.0).multiply(sinI2);

  580.             ey1 = qq.multiply(sinI2.multiply(-1.75).add(1.0));
  581.             eyx2 = qq.multiply(sinI2.multiply(-3.0).add(1.0));
  582.             eyy2 = qq.multiply(sinI2.multiply(2.0).subtract(1.5));
  583.             ey3 = qq.multiply(7.0 / 12.0).multiply(sinI2);
  584.             ey4 = qq.multiply(17.0 / 8.0).multiply(sinI2);

  586.             q  = cosI1.multiply(qq).negate();
  587.             rx1 = q.multiply(3.5);
  588.             ry1 = q.multiply(-2.5);
  589.             r2 = q.multiply(-0.5);
  590.             r3 =  q.multiply(7.0 / 6.0);
  591.             rl = g3 .multiply( cosI1).multiply(sinI2.multiply(-15.0).add(4.0)).subtract(
  592.                  g5.multiply(2.5).multiply(cosI1).multiply(sinI2.multiply(-42.0).add(4.0).add(sinI4.multiply(52.5))));

  594.             q = qq.multiply(0.5).multiply(sinI1).multiply(cosI1);
  595.             iy1 =  q;
  596.             ix1 = q.negate();
  597.             i2 =  q;
  598.             i3 =  q.multiply(7.0 / 3.0);
  599.             ih = g3.negate().multiply(cosI1).multiply(sinI2.multiply(-5.0).add(4)).add(
  600.                  g5.multiply(2.5).multiply(cosI1).multiply(sinI2.multiply(-14.0).add(4.0).add(sinI4.multiply(10.5))));
  601.             lx1 = qq.multiply(sinI2.multiply(-77.0 / 8.0).add(7.0));
  602.             ly1 = qq.multiply(sinI2.multiply(55.0 / 8.0).subtract(7.50));
  603.             l2 = qq.multiply(sinI2.multiply(1.25).subtract(0.5));
  604.             l3 = qq.multiply(sinI2.multiply(77.0 / 24.0).subtract(7.0 / 6.0));
  605.             ll = g3.multiply(sinI2.multiply(53.0).subtract(4.0).add(sinI4.multiply(-57.5))).add(
  606.                  g5.multiply(2.5).multiply(sinI2.multiply(-96.0).add(4.0).add(sinI4.multiply(269.5).subtract(sinI6.multiply(183.75)))));

  608.         }

  610.         /** Extrapolate an FieldOrbit<T> up to a specific target date.
  611.          * @param date target date for the FieldOrbit<T>
  612.          * @return propagated parameters
  613.          * @exception OrekitException if some parameters are out of bounds
  614.          */
  615.         public FieldDerivativeStructure<T>[] propagateParameters(final FieldAbsoluteDate<T> date)
  616.             throws OrekitException {
  617.             final Field<T> field = date.durationFrom(mean.getDate()).getField();
  618.             final T one = field.getOne();
  619.             final T zero = field.getZero();
  620.             // Keplerian evolution
  621.             final FieldDerivativeStructure<T> dt =
  622.                     factory.build(date.durationFrom(mean.getDate()), one, zero);
  623.             final FieldDerivativeStructure<T> xnot = dt.multiply(xnotDot);

  625.             // secular effects

  627.             // eccentricity
  628.             final FieldDerivativeStructure<T> x   = xnot.multiply(rdpom.add(rdpomp));
  629.             final FieldDerivativeStructure<T> cx  = x.cos();
  630.             final FieldDerivativeStructure<T> sx  = x.sin();
  631.             final FieldDerivativeStructure<T> exm = cx.multiply(mean.getCircularEx()).
  632.                                             add(sx.multiply(eps2.subtract(one.subtract(eps1).multiply(mean.getCircularEy()))));
  633.             final FieldDerivativeStructure<T> eym = sx.multiply(eps1.add(1.0).multiply(mean.getCircularEx())).
  634.                                             add(cx.multiply(mean.getCircularEy().subtract(eps2))).
  635.                                             add(eps2);
  636.             // no secular effect on inclination

  638.             // right ascension of ascending node
  639.             final FieldDerivativeStructure<T> omm =
  640.                             factory.build(normalizeAngle(mean.getRightAscensionOfAscendingNode().add(ommD.multiply(xnot.getValue())),
  641.                                                          zero.add(FastMath.PI)),
  642.                                           ommD.multiply(xnotDot),
  643.                                           zero);
  644.             // latitude argument
  645.             final FieldDerivativeStructure<T> xlm =
  646.                             factory.build(normalizeAngle(mean.getAlphaM().add(aMD.multiply(xnot.getValue())), zero.add(FastMath.PI)),
  647.                                           aMD.multiply(xnotDot),
  648.                                           zero);

  650.             // periodical terms
  651.             final FieldDerivativeStructure<T> cl1 = xlm.cos();
  652.             final FieldDerivativeStructure<T> sl1 = xlm.sin();
  653.             final FieldDerivativeStructure<T> cl2 = cl1.multiply(cl1).subtract(sl1.multiply(sl1));
  654.             final FieldDerivativeStructure<T> sl2 = cl1.multiply(sl1).add(sl1.multiply(cl1));
  655.             final FieldDerivativeStructure<T> cl3 = cl2.multiply(cl1).subtract(sl2.multiply(sl1));
  656.             final FieldDerivativeStructure<T> sl3 = cl2.multiply(sl1).add(sl2.multiply(cl1));
  657.             final FieldDerivativeStructure<T> cl4 = cl3.multiply(cl1).subtract(sl3.multiply(sl1));
  658.             final FieldDerivativeStructure<T> sl4 = cl3.multiply(sl1).add(sl3.multiply(cl1));
  659.             final FieldDerivativeStructure<T> cl5 = cl4.multiply(cl1).subtract(sl4.multiply(sl1));
  660.             final FieldDerivativeStructure<T> sl5 = cl4.multiply(sl1).add(sl4.multiply(cl1));
  661.             final FieldDerivativeStructure<T> cl6 = cl5.multiply(cl1).subtract(sl5.multiply(sl1));

  663.             final FieldDerivativeStructure<T> qh  = eym.subtract(eps2).multiply(kh);
  664.             final FieldDerivativeStructure<T> ql  = exm.multiply(kl);

  666.             final FieldDerivativeStructure<T> exmCl1 = exm.multiply(cl1);
  667.             final FieldDerivativeStructure<T> exmSl1 = exm.multiply(sl1);
  668.             final FieldDerivativeStructure<T> eymCl1 = eym.multiply(cl1);
  669.             final FieldDerivativeStructure<T> eymSl1 = eym.multiply(sl1);
  670.             final FieldDerivativeStructure<T> exmCl2 = exm.multiply(cl2);
  671.             final FieldDerivativeStructure<T> exmSl2 = exm.multiply(sl2);
  672.             final FieldDerivativeStructure<T> eymCl2 = eym.multiply(cl2);
  673.             final FieldDerivativeStructure<T> eymSl2 = eym.multiply(sl2);
  674.             final FieldDerivativeStructure<T> exmCl3 = exm.multiply(cl3);
  675.             final FieldDerivativeStructure<T> exmSl3 = exm.multiply(sl3);
  676.             final FieldDerivativeStructure<T> eymCl3 = eym.multiply(cl3);
  677.             final FieldDerivativeStructure<T> eymSl3 = eym.multiply(sl3);
  678.             final FieldDerivativeStructure<T> exmCl4 = exm.multiply(cl4);
  679.             final FieldDerivativeStructure<T> exmSl4 = exm.multiply(sl4);
  680.             final FieldDerivativeStructure<T> eymCl4 = eym.multiply(cl4);
  681.             final FieldDerivativeStructure<T> eymSl4 = eym.multiply(sl4);

  683.             // semi major axis
  684.             final FieldDerivativeStructure<T> rda = exmCl1.multiply(ax1).
  685.                                             add(eymSl1.multiply(ay1)).
  686.                                             add(sl1.multiply(as1)).
  687.                                             add(cl2.multiply(ac2)).
  688.                                             add(exmCl3.add(eymSl3).multiply(axy3)).
  689.                                             add(sl3.multiply(as3)).
  690.                                             add(cl4.multiply(ac4)).
  691.                                             add(sl5.multiply(as5)).
  692.                                             add(cl6.multiply(ac6));

  694.             // eccentricity
  695.             final FieldDerivativeStructure<T> rdex = cl1.multiply(ex1).
  696.                                              add(exmCl2.multiply(exx2)).
  697.                                              add(eymSl2.multiply(exy2)).
  698.                                              add(cl3.multiply(ex3)).
  699.                                              add(exmCl4.add(eymSl4).multiply(ex4));
  700.             final FieldDerivativeStructure<T> rdey = sl1.multiply(ey1).
  701.                                              add(exmSl2.multiply(eyx2)).
  702.                                              add(eymCl2.multiply(eyy2)).
  703.                                              add(sl3.multiply(ey3)).
  704.                                              add(exmSl4.subtract(eymCl4).multiply(ey4));

  706.             // ascending node
  707.             final FieldDerivativeStructure<T> rdom = exmSl1.multiply(rx1).
  708.                                              add(eymCl1.multiply(ry1)).
  709.                                              add(sl2.multiply(r2)).
  710.                                              add(eymCl3.subtract(exmSl3).multiply(r3)).
  711.                                              add(ql.multiply(rl));

  713.             // inclination
  714.             final FieldDerivativeStructure<T> rdxi = eymSl1.multiply(iy1).
  715.                                              add(exmCl1.multiply(ix1)).
  716.                                              add(cl2.multiply(i2)).
  717.                                              add(exmCl3.add(eymSl3).multiply(i3)).
  718.                                              add(qh.multiply(ih));

  720.             // latitude argument
  721.             final FieldDerivativeStructure<T> rdxl = exmSl1.multiply(lx1).
  722.                                              add(eymCl1.multiply(ly1)).
  723.                                              add(sl2.multiply(l2)).
  724.                                              add(exmSl3.subtract(eymCl3).multiply(l3)).
  725.                                              add(ql.multiply(ll));
  726.             // osculating parameters
  727.             final FieldDerivativeStructure<T>[] FTD = MathArrays.buildArray(rdxl.getField(), 6);

  729.             FTD[0] = rda.add(1.0).multiply(mean.getA());
  730.             FTD[1] = rdex.add(exm);
  731.             FTD[2] = rdey.add(eym);
  732.             FTD[3] = rdxi.add(xim);
  733.             FTD[4] = rdom.add(omm);
  734.             FTD[5] = rdxl.add(xlm);
  735.             return FTD;

  737.         }

  739.     }

  741.     /** Convert circular parameters <em>with derivatives</em> to Cartesian coordinates.
  742.      * @param date date of the FieldOrbit<T>al parameters
  743.      * @param parameters circular parameters (a, ex, ey, i, raan, alphaM)
  744.      * @return Cartesian coordinates consistent with values and derivatives
  745.      */
  746.     private TimeStampedFieldPVCoordinates<T> toCartesian(final FieldAbsoluteDate<T> date, final FieldDerivativeStructure<T>[] parameters) {

  748.         // evaluate coordinates in the FieldOrbit<T> canonical reference frame
  749.         final FieldDerivativeStructure<T> cosOmega = parameters[4].cos();
  750.         final FieldDerivativeStructure<T> sinOmega = parameters[4].sin();
  751.         final FieldDerivativeStructure<T> cosI     = parameters[3].cos();
  752.         final FieldDerivativeStructure<T> sinI     = parameters[3].sin();
  753.         final FieldDerivativeStructure<T> alphaE   = meanToEccentric(parameters[5], parameters[1], parameters[2]);
  754.         final FieldDerivativeStructure<T> cosAE    = alphaE.cos();
  755.         final FieldDerivativeStructure<T> sinAE    = alphaE.sin();
  756.         final FieldDerivativeStructure<T> ex2      = parameters[1].multiply(parameters[1]);
  757.         final FieldDerivativeStructure<T> ey2      = parameters[2].multiply(parameters[2]);
  758.         final FieldDerivativeStructure<T> exy      = parameters[1].multiply(parameters[2]);
  759.         final FieldDerivativeStructure<T> q        = ex2.add(ey2).subtract(1).negate().sqrt();
  760.         final FieldDerivativeStructure<T> beta     = q.add(1).reciprocal();
  761.         final FieldDerivativeStructure<T> bx2      = beta.multiply(ex2);
  762.         final FieldDerivativeStructure<T> by2      = beta.multiply(ey2);
  763.         final FieldDerivativeStructure<T> bxy      = beta.multiply(exy);
  764.         final FieldDerivativeStructure<T> u        = bxy.multiply(sinAE).subtract(parameters[1].add(by2.subtract(1).multiply(cosAE)));
  765.         final FieldDerivativeStructure<T> v        = bxy.multiply(cosAE).subtract(parameters[2].add(bx2.subtract(1).multiply(sinAE)));
  766.         final FieldDerivativeStructure<T> x        = parameters[0].multiply(u);
  767.         final FieldDerivativeStructure<T> y        = parameters[0].multiply(v);

  769.         // canonical FieldOrbit<T> reference frame
  770.         final FieldVector3D<FieldDerivativeStructure<T>> p =
  771.                 new FieldVector3D<FieldDerivativeStructure<T>>(x.multiply(cosOmega).subtract(y.multiply(cosI.multiply(sinOmega))),
  772.                                                        x.multiply(sinOmega).add(y.multiply(cosI.multiply(cosOmega))),
  773.                                                        y.multiply(sinI));

  775.         // dispatch derivatives
  776.         final FieldVector3D<T> p0 = new FieldVector3D<>(p.getX().getValue(),
  777.                                                         p.getY().getValue(),
  778.                                                         p.getZ().getValue());
  779.         final FieldVector3D<T> p1 = new FieldVector3D<>(p.getX().getPartialDerivative(1),
  780.                                                         p.getY().getPartialDerivative(1),
  781.                                                         p.getZ().getPartialDerivative(1));
  782.         final FieldVector3D<T> p2 = new FieldVector3D<>(p.getX().getPartialDerivative(2),
  783.                                                         p.getY().getPartialDerivative(2),
  784.                                                         p.getZ().getPartialDerivative(2));
  785.         return new TimeStampedFieldPVCoordinates<>(date, p0, p1, p2);

  787.     }

  789.     /** Computes the eccentric latitude argument from the mean latitude argument.
  790.      * @param alphaM = M + Ω mean latitude argument (rad)
  791.      * @param ex e cos(Ω), first component of circular eccentricity vector
  792.      * @param ey e sin(Ω), second component of circular eccentricity vector
  793.      * @return the eccentric latitude argument.
  794.      */
  795.     private FieldDerivativeStructure<T> meanToEccentric(final FieldDerivativeStructure<T> alphaM,
  796.                                                 final FieldDerivativeStructure<T> ex,
  797.                                                 final FieldDerivativeStructure<T> ey) {
  798.         // Generalization of Kepler equation to circular parameters
  799.         // with alphaE = PA + E and
  800.         //      alphaM = PA + M = alphaE - ex.sin(alphaE) + ey.cos(alphaE)
  801.         FieldDerivativeStructure<T> alphaE        = alphaM;
  802.         FieldDerivativeStructure<T> shift         = alphaM.getField().getZero();
  803.         FieldDerivativeStructure<T> alphaEMalphaM = alphaM.getField().getZero();
  804.         FieldDerivativeStructure<T> cosAlphaE     = alphaE.cos();
  805.         FieldDerivativeStructure<T> sinAlphaE     = alphaE.sin();
  806.         int                 iter          = 0;
  807.         do {
  808.             final FieldDerivativeStructure<T> f2 = ex.multiply(sinAlphaE).subtract(ey.multiply(cosAlphaE));
  809.             final FieldDerivativeStructure<T> f1 = alphaM.getField().getOne().subtract(ex.multiply(cosAlphaE)).subtract(ey.multiply(sinAlphaE));
  810.             final FieldDerivativeStructure<T> f0 = alphaEMalphaM.subtract(f2);

  812.             final FieldDerivativeStructure<T> f12 = f1.multiply(2);
  813.             shift = f0.multiply(f12).divide(f1.multiply(f12).subtract(f0.multiply(f2)));

  815.             alphaEMalphaM  = alphaEMalphaM.subtract(shift);
  816.             alphaE         = alphaM.add(alphaEMalphaM);
  817.             cosAlphaE      = alphaE.cos();
  818.             sinAlphaE      = alphaE.sin();

  820.         } while ((++iter < 50) && (FastMath.abs(shift.getValue().getReal()) > 1.0e-12));

  822.         return alphaE;

  824.     }

  826.     /** {@inheritDoc} */
  827.     protected T getMass(final FieldAbsoluteDate<T> date) {
  828.         return models.get(date).mass;
  829.     }
  830.     /**
  831.      * Normalize an angle in a 2&pi; wide interval around a center value.
  832.      * <p>This method has three main uses:</p>
  833.      * <ul>
  834.      *   <li>normalize an angle between 0 and 2&pi;:<br/>
  835.      *       {@code a = MathUtils.normalizeAngle(a, FastMath.PI);}</li>
  836.      *   <li>normalize an angle between -&pi; and +&pi;<br/>
  837.      *       {@code a = MathUtils.normalizeAngle(a, 0.0);}</li>
  838.      *   <li>compute the angle between two defining angular positions:<br>
  839.      *       {@code angle = MathUtils.normalizeAngle(end, start) - start;}</li>
  840.      * </ul>
  841.      * <p>Note that due to numerical accuracy and since &pi; cannot be represented
  842.      * exactly, the result interval is <em>closed</em>, it cannot be half-closed
  843.      * as would be more satisfactory in a purely mathematical view.</p>
  844.      * @param a angle to normalize
  845.      * @param center center of the desired 2&pi; interval for the result
  846.      * @param <T> the type of the field elements
  847.      * @return a-2k&pi; with integer k and center-&pi; &lt;= a-2k&pi; &lt;= center+&pi;
  848.      * @since 1.2
  849.      */
  850.     public static <T extends RealFieldElement<T>> T normalizeAngle(final T a, final T center) {
  851.         return a.subtract(2 * FastMath.PI * FastMath.floor((a.getReal() + FastMath.PI - center.getReal()) / (2 * FastMath.PI)));
  852.     }


  855. }
Created with JaCoCo 0.7.9.201702052155