HolmesFeatherstoneAttractionModel.java

  1. /* Copyright 2002-2022 CS GROUP
  2.  * Licensed to CS GROUP (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.forces.gravity;


  18. import java.util.Collections;
  19. import java.util.List;
  20. import java.util.stream.Stream;

  21. import org.hipparchus.CalculusFieldElement;
  22. import org.hipparchus.Field;
  23. import org.hipparchus.analysis.differentiation.DerivativeStructure;
  24. import org.hipparchus.analysis.differentiation.Gradient;
  25. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  26. import org.hipparchus.geometry.euclidean.threed.SphericalCoordinates;
  27. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  28. import org.hipparchus.linear.Array2DRowRealMatrix;
  29. import org.hipparchus.linear.RealMatrix;
  30. import org.hipparchus.util.FastMath;
  31. import org.hipparchus.util.MathArrays;
  32. import org.orekit.forces.AbstractForceModel;
  33. import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
  34. import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider.NormalizedSphericalHarmonics;
  35. import org.orekit.forces.gravity.potential.TideSystem;
  36. import org.orekit.forces.gravity.potential.TideSystemProvider;
  37. import org.orekit.frames.Frame;
  38. import org.orekit.frames.StaticTransform;
  39. import org.orekit.propagation.FieldSpacecraftState;
  40. import org.orekit.propagation.SpacecraftState;
  41. import org.orekit.propagation.events.EventDetector;
  42. import org.orekit.propagation.events.FieldEventDetector;
  43. import org.orekit.time.AbsoluteDate;
  44. import org.orekit.time.FieldAbsoluteDate;
  45. import org.orekit.utils.FieldPVCoordinates;
  46. import org.orekit.utils.ParameterDriver;

  47. /** This class represents the gravitational field of a celestial body.
  48.  * <p>
  49.  * The algorithm implemented in this class has been designed by S. A. Holmes
  50.  * and W. E. Featherstone from Department of Spatial Sciences, Curtin University
  51.  * of Technology, Perth, Australia. It is described in their 2002 paper: <a
  52.  * href="https://www.researchgate.net/publication/226460594_A_unified_approach_to_the_Clenshaw_summation_and_the_recursive_computation_of_very_high_degree_and_order_normalised_associated_Legendre_functions">
  53.  * A unified approach to he Clenshaw summation and the recursive computation of
  54.  * very high degree and order normalised associated Legendre functions</a>
  55.  * (Journal of Geodesy (2002) 76: 279–299).
  56.  * </p>
  57.  * <p>
  58.  * This model directly uses normalized coefficients and stable recursion algorithms
  59.  * so it is more suited to high degree gravity fields than the classical Cunningham
  60.  * Droziner models which use un-normalized coefficients.
  61.  * </p>
  62.  * <p>
  63.  * Among the different algorithms presented in Holmes and Featherstone paper, this
  64.  * class implements the <em>modified forward row method</em>. All recursion coefficients
  65.  * are precomputed and stored for greater performance. This caching was suggested in the
  66.  * paper but not used due to the large memory requirements. Since 2002, even low end
  67.  * computers and mobile devices do have sufficient memory so this caching has become
  68.  * feasible nowadays.
  69.  * <p>
  70.  * @author Luc Maisonobe
  71.  * @since 6.0
  72.  */

  73. public class HolmesFeatherstoneAttractionModel extends AbstractForceModel implements TideSystemProvider {

  74.     /** Exponent scaling to avoid floating point overflow.
  75.      * <p>The paper uses 10^280, we prefer a power of two to preserve accuracy thanks to
  76.      * {@link FastMath#scalb(double, int)}, so we use 2^930 which has the same order of magnitude.
  77.      */
  78.     private static final int SCALING = 930;

  79.     /** Central attraction scaling factor.
  80.      * <p>
  81.      * We use a power of 2 to avoid numeric noise introduction
  82.      * in the multiplications/divisions sequences.
  83.      * </p>
  84.      */
  85.     private static final double MU_SCALE = FastMath.scalb(1.0, 32);

  86.     /** Driver for gravitational parameter. */
  87.     private final ParameterDriver gmParameterDriver;

  88.     /** Provider for the spherical harmonics. */
  89.     private final NormalizedSphericalHarmonicsProvider provider;

  90.     /** Rotating body. */
  91.     private final Frame bodyFrame;

  92.     /** Recursion coefficients g<sub>n,m</sub>/√j. */
  93.     private final double[] gnmOj;

  94.     /** Recursion coefficients h<sub>n,m</sub>/√j. */
  95.     private final double[] hnmOj;

  96.     /** Recursion coefficients e<sub>n,m</sub>. */
  97.     private final double[] enm;

  98.     /** Scaled sectorial Pbar<sub>m,m</sub>/u<sup>m</sup> &times; 2<sup>-SCALING</sup>. */
  99.     private final double[] sectorial;

  100.     /** Creates a new instance.
  101.      * @param centralBodyFrame rotating body frame
  102.      * @param provider provider for spherical harmonics
  103.      * @since 6.0
  104.      */
  105.     public HolmesFeatherstoneAttractionModel(final Frame centralBodyFrame,
  106.                                              final NormalizedSphericalHarmonicsProvider provider) {

  107.         gmParameterDriver = new ParameterDriver(NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT,
  108.                                                 provider.getMu(), MU_SCALE, 0.0, Double.POSITIVE_INFINITY);

  109.         this.provider  = provider;
  110.         this.bodyFrame = centralBodyFrame;

  111.         // the pre-computed arrays hold coefficients from triangular arrays in a single
  112.         // storing neither diagonal elements (n = m) nor the non-diagonal element n=1, m=0
  113.         final int degree = provider.getMaxDegree();
  114.         final int size = FastMath.max(0, degree * (degree + 1) / 2 - 1);
  115.         gnmOj = new double[size];
  116.         hnmOj = new double[size];
  117.         enm   = new double[size];

  118.         // pre-compute the recursion coefficients corresponding to equations 19 and 22
  119.         // from Holmes and Featherstone paper
  120.         // for cache efficiency, elements are stored in the same order they will be used
  121.         // later on, i.e. from rightmost column to leftmost column
  122.         int index = 0;
  123.         for (int m = degree; m >= 0; --m) {
  124.             final int j = (m == 0) ? 2 : 1;
  125.             for (int n = FastMath.max(2, m + 1); n <= degree; ++n) {
  126.                 final double f = (n - m) * (n + m + 1);
  127.                 gnmOj[index] = 2 * (m + 1) / FastMath.sqrt(j * f);
  128.                 hnmOj[index] = FastMath.sqrt((n + m + 2) * (n - m - 1) / (j * f));
  129.                 enm[index]   = FastMath.sqrt(f / j);
  130.                 ++index;
  131.             }
  132.         }

  133.         // scaled sectorial terms corresponding to equation 28 in Holmes and Featherstone paper
  134.         sectorial    = new double[degree + 1];
  135.         sectorial[0] = FastMath.scalb(1.0, -SCALING);
  136.         if (degree > 0) {
  137.             sectorial[1] = FastMath.sqrt(3) * sectorial[0];
  138.         }
  139.         for (int m = 2; m < sectorial.length; ++m) {
  140.             sectorial[m] = FastMath.sqrt((2 * m + 1) / (2.0 * m)) * sectorial[m - 1];
  141.         }

  142.     }

  143.     /** {@inheritDoc} */
  144.     @Override
  145.     public boolean dependsOnPositionOnly() {
  146.         return true;
  147.     }

  148.     /** {@inheritDoc} */
  149.     public TideSystem getTideSystem() {
  150.         return provider.getTideSystem();
  151.     }

  152.     /** Get the central attraction coefficient μ.
  153.      * @return mu central attraction coefficient (m³/s²)
  154.      */
  155.     public double getMu() {
  156.         return gmParameterDriver.getValue();
  157.     }

  158.     /** Compute the value of the gravity field.
  159.      * @param date current date
  160.      * @param position position at which gravity field is desired in body frame
  161.      * @param mu central attraction coefficient to use
  162.      * @return value of the gravity field (central and non-central parts summed together)
  163.      */
  164.     public double value(final AbsoluteDate date, final Vector3D position,
  165.                         final double mu) {
  166.         return mu / position.getNorm() + nonCentralPart(date, position, mu);
  167.     }

  168.     /** Compute the non-central part of the gravity field.
  169.      * @param date current date
  170.      * @param position position at which gravity field is desired in body frame
  171.      * @param mu central attraction coefficient to use
  172.      * @return value of the non-central part of the gravity field
  173.      */
  174.     public double nonCentralPart(final AbsoluteDate date, final Vector3D position, final double mu) {

  175.         final int degree = provider.getMaxDegree();
  176.         final int order  = provider.getMaxOrder();
  177.         final NormalizedSphericalHarmonics harmonics = provider.onDate(date);

  178.         // allocate the columns for recursion
  179.         double[] pnm0Plus2 = new double[degree + 1];
  180.         double[] pnm0Plus1 = new double[degree + 1];
  181.         double[] pnm0      = new double[degree + 1];

  182.         // compute polar coordinates
  183.         final double x   = position.getX();
  184.         final double y   = position.getY();
  185.         final double z   = position.getZ();
  186.         final double x2  = x * x;
  187.         final double y2  = y * y;
  188.         final double z2  = z * z;
  189.         final double r2  = x2 + y2 + z2;
  190.         final double r   = FastMath.sqrt (r2);
  191.         final double rho = FastMath.sqrt(x2 + y2);
  192.         final double t   = z / r;   // cos(theta), where theta is the polar angle
  193.         final double u   = rho / r; // sin(theta), where theta is the polar angle
  194.         final double tOu = z / rho;

  195.         // compute distance powers
  196.         final double[] aOrN = createDistancePowersArray(provider.getAe() / r);

  197.         // compute longitude cosines/sines
  198.         final double[][] cosSinLambda = createCosSinArrays(position.getX() / rho, position.getY() / rho);

  199.         // outer summation over order
  200.         int    index = 0;
  201.         double value = 0;
  202.         for (int m = degree; m >= 0; --m) {

  203.             // compute tesseral terms without derivatives
  204.             index = computeTesseral(m, degree, index, t, u, tOu,
  205.                                     pnm0Plus2, pnm0Plus1, null, pnm0, null, null);

  206.             if (m <= order) {
  207.                 // compute contribution of current order to field (equation 5 of the paper)

  208.                 // inner summation over degree, for fixed order
  209.                 double sumDegreeS        = 0;
  210.                 double sumDegreeC        = 0;
  211.                 for (int n = FastMath.max(2, m); n <= degree; ++n) {
  212.                     sumDegreeS += pnm0[n] * aOrN[n] * harmonics.getNormalizedSnm(n, m);
  213.                     sumDegreeC += pnm0[n] * aOrN[n] * harmonics.getNormalizedCnm(n, m);
  214.                 }

  215.                 // contribution to outer summation over order
  216.                 value = value * u + cosSinLambda[1][m] * sumDegreeS + cosSinLambda[0][m] * sumDegreeC;

  217.             }

  218.             // rotate the recursion arrays
  219.             final double[] tmp = pnm0Plus2;
  220.             pnm0Plus2 = pnm0Plus1;
  221.             pnm0Plus1 = pnm0;
  222.             pnm0      = tmp;

  223.         }

  224.         // scale back
  225.         value = FastMath.scalb(value, SCALING);

  226.         // apply the global mu/r factor
  227.         return mu * value / r;

  228.     }

  229.     /** Compute the gradient of the non-central part of the gravity field.
  230.      * @param date current date
  231.      * @param position position at which gravity field is desired in body frame
  232.      * @param mu central attraction coefficient to use
  233.      * @return gradient of the non-central part of the gravity field
  234.      */
  235.     public double[] gradient(final AbsoluteDate date, final Vector3D position, final double mu) {

  236.         final int degree = provider.getMaxDegree();
  237.         final int order  = provider.getMaxOrder();
  238.         final NormalizedSphericalHarmonics harmonics = provider.onDate(date);

  239.         // allocate the columns for recursion
  240.         double[] pnm0Plus2  = new double[degree + 1];
  241.         double[] pnm0Plus1  = new double[degree + 1];
  242.         double[] pnm0       = new double[degree + 1];
  243.         final double[] pnm1 = new double[degree + 1];

  244.         // compute polar coordinates
  245.         final double x    = position.getX();
  246.         final double y    = position.getY();
  247.         final double z    = position.getZ();
  248.         final double x2   = x * x;
  249.         final double y2   = y * y;
  250.         final double z2   = z * z;
  251.         final double r2   = x2 + y2 + z2;
  252.         final double r    = FastMath.sqrt (r2);
  253.         final double rho2 = x2 + y2;
  254.         final double rho  = FastMath.sqrt(rho2);
  255.         final double t    = z / r;   // cos(theta), where theta is the polar angle
  256.         final double u    = rho / r; // sin(theta), where theta is the polar angle
  257.         final double tOu  = z / rho;

  258.         // compute distance powers
  259.         final double[] aOrN = createDistancePowersArray(provider.getAe() / r);

  260.         // compute longitude cosines/sines
  261.         final double[][] cosSinLambda = createCosSinArrays(position.getX() / rho, position.getY() / rho);

  262.         // outer summation over order
  263.         int    index = 0;
  264.         double value = 0;
  265.         final double[] gradient = new double[3];
  266.         for (int m = degree; m >= 0; --m) {

  267.             // compute tesseral terms with derivatives
  268.             index = computeTesseral(m, degree, index, t, u, tOu,
  269.                                     pnm0Plus2, pnm0Plus1, null, pnm0, pnm1, null);

  270.             if (m <= order) {
  271.                 // compute contribution of current order to field (equation 5 of the paper)

  272.                 // inner summation over degree, for fixed order
  273.                 double sumDegreeS        = 0;
  274.                 double sumDegreeC        = 0;
  275.                 double dSumDegreeSdR     = 0;
  276.                 double dSumDegreeCdR     = 0;
  277.                 double dSumDegreeSdTheta = 0;
  278.                 double dSumDegreeCdTheta = 0;
  279.                 for (int n = FastMath.max(2, m); n <= degree; ++n) {
  280.                     final double qSnm  = aOrN[n] * harmonics.getNormalizedSnm(n, m);
  281.                     final double qCnm  = aOrN[n] * harmonics.getNormalizedCnm(n, m);
  282.                     final double nOr   = n / r;
  283.                     final double s0    = pnm0[n] * qSnm;
  284.                     final double c0    = pnm0[n] * qCnm;
  285.                     final double s1    = pnm1[n] * qSnm;
  286.                     final double c1    = pnm1[n] * qCnm;
  287.                     sumDegreeS        += s0;
  288.                     sumDegreeC        += c0;
  289.                     dSumDegreeSdR     -= nOr * s0;
  290.                     dSumDegreeCdR     -= nOr * c0;
  291.                     dSumDegreeSdTheta += s1;
  292.                     dSumDegreeCdTheta += c1;
  293.                 }

  294.                 // contribution to outer summation over order
  295.                 // beware that we need to order gradient using the mathematical conventions
  296.                 // compliant with the SphericalCoordinates class, so our lambda is its theta
  297.                 // (and hence at index 1) and our theta is its phi (and hence at index 2)
  298.                 final double sML = cosSinLambda[1][m];
  299.                 final double cML = cosSinLambda[0][m];
  300.                 value            = value       * u + sML * sumDegreeS        + cML * sumDegreeC;
  301.                 gradient[0]      = gradient[0] * u + sML * dSumDegreeSdR     + cML * dSumDegreeCdR;
  302.                 gradient[1]      = gradient[1] * u + m * (cML * sumDegreeS - sML * sumDegreeC);
  303.                 gradient[2]      = gradient[2] * u + sML * dSumDegreeSdTheta + cML * dSumDegreeCdTheta;

  304.             }

  305.             // rotate the recursion arrays
  306.             final double[] tmp = pnm0Plus2;
  307.             pnm0Plus2 = pnm0Plus1;
  308.             pnm0Plus1 = pnm0;
  309.             pnm0      = tmp;

  310.         }

  311.         // scale back
  312.         value       = FastMath.scalb(value,       SCALING);
  313.         gradient[0] = FastMath.scalb(gradient[0], SCALING);
  314.         gradient[1] = FastMath.scalb(gradient[1], SCALING);
  315.         gradient[2] = FastMath.scalb(gradient[2], SCALING);

  316.         // apply the global mu/r factor
  317.         final double muOr = mu / r;
  318.         value            *= muOr;
  319.         gradient[0]       = muOr * gradient[0] - value / r;
  320.         gradient[1]      *= muOr;
  321.         gradient[2]      *= muOr;

  322.         // convert gradient from spherical to Cartesian
  323.         return new SphericalCoordinates(position).toCartesianGradient(gradient);

  324.     }

  325.     /** Compute the gradient of the non-central part of the gravity field.
  326.      * @param date current date
  327.      * @param position position at which gravity field is desired in body frame
  328.      * @param mu central attraction coefficient to use
  329.      * @param <T> type of field used
  330.      * @return gradient of the non-central part of the gravity field
  331.      */
  332.     public <T extends CalculusFieldElement<T>> T[] gradient(final FieldAbsoluteDate<T> date, final FieldVector3D<T> position,
  333.                                                         final T mu) {

  334.         final int degree = provider.getMaxDegree();
  335.         final int order  = provider.getMaxOrder();
  336.         final NormalizedSphericalHarmonics harmonics = provider.onDate(date.toAbsoluteDate());
  337.         final T zero = date.getField().getZero();
  338.         // allocate the columns for recursion
  339.         T[] pnm0Plus2  = MathArrays.buildArray(date.getField(), degree + 1);
  340.         T[] pnm0Plus1  = MathArrays.buildArray(date.getField(), degree + 1);
  341.         T[] pnm0       = MathArrays.buildArray(date.getField(), degree + 1);
  342.         final T[] pnm1 = MathArrays.buildArray(date.getField(), degree + 1);

  343.         // compute polar coordinates
  344.         final T x    = position.getX();
  345.         final T y    = position.getY();
  346.         final T z    = position.getZ();
  347.         final T x2   = x.multiply(x);
  348.         final T y2   = y.multiply(y);
  349.         final T z2   = z.multiply(z);
  350.         final T r2   = x2.add(y2).add(z2);
  351.         final T r    = r2.sqrt();
  352.         final T rho2 = x2.add(y2);
  353.         final T rho  = rho2.sqrt();
  354.         final T t    = z.divide(r);   // cos(theta), where theta is the polar angle
  355.         final T u    = rho.divide(r); // sin(theta), where theta is the polar angle
  356.         final T tOu  = z.divide(rho);

  357.         // compute distance powers
  358.         final T[] aOrN = createDistancePowersArray(r.reciprocal().multiply(provider.getAe()));

  359.         // compute longitude cosines/sines
  360.         final T[][] cosSinLambda = createCosSinArrays(rho.reciprocal().multiply(position.getX()), rho.reciprocal().multiply(position.getY()));
  361.         // outer summation over order
  362.         int    index = 0;
  363.         T value = zero;
  364.         final T[] gradient = MathArrays.buildArray(zero.getField(), 3);
  365.         for (int m = degree; m >= 0; --m) {

  366.             // compute tesseral terms with derivatives
  367.             index = computeTesseral(m, degree, index, t, u, tOu,
  368.                                     pnm0Plus2, pnm0Plus1, null, pnm0, pnm1, null);
  369.             if (m <= order) {
  370.                 // compute contribution of current order to field (equation 5 of the paper)

  371.                 // inner summation over degree, for fixed order
  372.                 T sumDegreeS        = zero;
  373.                 T sumDegreeC        = zero;
  374.                 T dSumDegreeSdR     = zero;
  375.                 T dSumDegreeCdR     = zero;
  376.                 T dSumDegreeSdTheta = zero;
  377.                 T dSumDegreeCdTheta = zero;
  378.                 for (int n = FastMath.max(2, m); n <= degree; ++n) {
  379.                     final T qSnm  = aOrN[n].multiply(harmonics.getNormalizedSnm(n, m));
  380.                     final T qCnm  = aOrN[n].multiply(harmonics.getNormalizedCnm(n, m));
  381.                     final T nOr   = r.reciprocal().multiply(n);
  382.                     final T s0    = pnm0[n].multiply(qSnm);
  383.                     final T c0    = pnm0[n].multiply(qCnm);
  384.                     final T s1    = pnm1[n].multiply(qSnm);
  385.                     final T c1    = pnm1[n].multiply(qCnm);
  386.                     sumDegreeS        = sumDegreeS       .add(s0);
  387.                     sumDegreeC        = sumDegreeC       .add(c0);
  388.                     dSumDegreeSdR     = dSumDegreeSdR    .subtract(nOr.multiply(s0));
  389.                     dSumDegreeCdR     = dSumDegreeCdR    .subtract(nOr.multiply(c0));
  390.                     dSumDegreeSdTheta = dSumDegreeSdTheta.add(s1);
  391.                     dSumDegreeCdTheta = dSumDegreeCdTheta.add(c1);
  392.                 }

  393.                 // contribution to outer summation over order
  394.                 // beware that we need to order gradient using the mathematical conventions
  395.                 // compliant with the SphericalCoordinates class, so our lambda is its theta
  396.                 // (and hence at index 1) and our theta is its phi (and hence at index 2)
  397.                 final T sML = cosSinLambda[1][m];
  398.                 final T cML = cosSinLambda[0][m];
  399.                 value            = value      .multiply(u).add(sML.multiply(sumDegreeS   )).add(cML.multiply(sumDegreeC));
  400.                 gradient[0]      = gradient[0].multiply(u).add(sML.multiply(dSumDegreeSdR)).add(cML.multiply(dSumDegreeCdR));
  401.                 gradient[1]      = gradient[1].multiply(u).add(cML.multiply(sumDegreeS).subtract(sML.multiply(sumDegreeC)).multiply(m));
  402.                 gradient[2]      = gradient[2].multiply(u).add(sML.multiply(dSumDegreeSdTheta)).add(cML.multiply(dSumDegreeCdTheta));
  403.             }
  404.             // rotate the recursion arrays
  405.             final T[] tmp = pnm0Plus2;
  406.             pnm0Plus2 = pnm0Plus1;
  407.             pnm0Plus1 = pnm0;
  408.             pnm0      = tmp;

  409.         }
  410.         // scale back
  411.         value       = value.scalb(SCALING);
  412.         gradient[0] = gradient[0].scalb(SCALING);
  413.         gradient[1] = gradient[1].scalb(SCALING);
  414.         gradient[2] = gradient[2].scalb(SCALING);

  415.         // apply the global mu/r factor
  416.         final T muOr = r.reciprocal().multiply(mu);
  417.         value            = value.multiply(muOr);
  418.         gradient[0]       = muOr.multiply(gradient[0]).subtract(value.divide(r));
  419.         gradient[1]      = gradient[1].multiply(muOr);
  420.         gradient[2]      = gradient[2].multiply(muOr);

  421.         // convert gradient from spherical to Cartesian
  422.         // Cartesian coordinates
  423.         // remaining spherical coordinates
  424.         final T rPos     = position.getNorm();
  425.         // intermediate variables
  426.         final T xPos    = position.getX();
  427.         final T yPos    = position.getY();
  428.         final T zPos    = position.getZ();
  429.         final T rho2Pos = x.multiply(x).add(y.multiply(y));
  430.         final T rhoPos  = rho2.sqrt();
  431.         final T r2Pos   = rho2.add(z.multiply(z));

  432.         final T[][] jacobianPos = MathArrays.buildArray(zero.getField(), 3, 3);

  433.         // row representing the gradient of r
  434.         jacobianPos[0][0] = xPos.divide(rPos);
  435.         jacobianPos[0][1] = yPos.divide(rPos);
  436.         jacobianPos[0][2] = zPos.divide(rPos);

  437.         // row representing the gradient of theta
  438.         jacobianPos[1][0] =  yPos.negate().divide(rho2Pos);
  439.         jacobianPos[1][1] =  xPos.divide(rho2Pos);
  440.         // jacobian[1][2] is already set to 0 at allocation time

  441.         // row representing the gradient of phi
  442.         jacobianPos[2][0] = xPos.multiply(zPos).divide(rhoPos.multiply(r2Pos));
  443.         jacobianPos[2][1] = yPos.multiply(zPos).divide(rhoPos.multiply(r2Pos));
  444.         jacobianPos[2][2] = rhoPos.negate().divide(r2Pos);
  445.         final T[] cartGradPos = MathArrays.buildArray(zero.getField(), 3);
  446.         cartGradPos[0] = gradient[0].multiply(jacobianPos[0][0]).add(gradient[1].multiply(jacobianPos[1][0])).add(gradient[2].multiply(jacobianPos[2][0]));
  447.         cartGradPos[1] = gradient[0].multiply(jacobianPos[0][1]).add(gradient[1].multiply(jacobianPos[1][1])).add(gradient[2].multiply(jacobianPos[2][1]));
  448.         cartGradPos[2] = gradient[0].multiply(jacobianPos[0][2])                                      .add(gradient[2].multiply(jacobianPos[2][2]));
  449.         return cartGradPos;

  450.     }

  451.     /** Compute both the gradient and the hessian of the non-central part of the gravity field.
  452.      * @param date current date
  453.      * @param position position at which gravity field is desired in body frame
  454.      * @param mu central attraction coefficient to use
  455.      * @return gradient and hessian of the non-central part of the gravity field
  456.      */
  457.     private GradientHessian gradientHessian(final AbsoluteDate date, final Vector3D position, final double mu) {

  458.         final int degree = provider.getMaxDegree();
  459.         final int order  = provider.getMaxOrder();
  460.         final NormalizedSphericalHarmonics harmonics = provider.onDate(date);

  461.         // allocate the columns for recursion
  462.         double[] pnm0Plus2  = new double[degree + 1];
  463.         double[] pnm0Plus1  = new double[degree + 1];
  464.         double[] pnm0       = new double[degree + 1];
  465.         double[] pnm1Plus1  = new double[degree + 1];
  466.         double[] pnm1       = new double[degree + 1];
  467.         final double[] pnm2 = new double[degree + 1];

  468.         // compute polar coordinates
  469.         final double x    = position.getX();
  470.         final double y    = position.getY();
  471.         final double z    = position.getZ();
  472.         final double x2   = x * x;
  473.         final double y2   = y * y;
  474.         final double z2   = z * z;
  475.         final double r2   = x2 + y2 + z2;
  476.         final double r    = FastMath.sqrt (r2);
  477.         final double rho2 = x2 + y2;
  478.         final double rho  = FastMath.sqrt(rho2);
  479.         final double t    = z / r;   // cos(theta), where theta is the polar angle
  480.         final double u    = rho / r; // sin(theta), where theta is the polar angle
  481.         final double tOu  = z / rho;

  482.         // compute distance powers
  483.         final double[] aOrN = createDistancePowersArray(provider.getAe() / r);

  484.         // compute longitude cosines/sines
  485.         final double[][] cosSinLambda = createCosSinArrays(position.getX() / rho, position.getY() / rho);

  486.         // outer summation over order
  487.         int    index = 0;
  488.         double value = 0;
  489.         final double[]   gradient = new double[3];
  490.         final double[][] hessian  = new double[3][3];
  491.         for (int m = degree; m >= 0; --m) {

  492.             // compute tesseral terms
  493.             index = computeTesseral(m, degree, index, t, u, tOu,
  494.                                     pnm0Plus2, pnm0Plus1, pnm1Plus1, pnm0, pnm1, pnm2);

  495.             if (m <= order) {
  496.                 // compute contribution of current order to field (equation 5 of the paper)

  497.                 // inner summation over degree, for fixed order
  498.                 double sumDegreeS               = 0;
  499.                 double sumDegreeC               = 0;
  500.                 double dSumDegreeSdR            = 0;
  501.                 double dSumDegreeCdR            = 0;
  502.                 double dSumDegreeSdTheta        = 0;
  503.                 double dSumDegreeCdTheta        = 0;
  504.                 double d2SumDegreeSdRdR         = 0;
  505.                 double d2SumDegreeSdRdTheta     = 0;
  506.                 double d2SumDegreeSdThetadTheta = 0;
  507.                 double d2SumDegreeCdRdR         = 0;
  508.                 double d2SumDegreeCdRdTheta     = 0;
  509.                 double d2SumDegreeCdThetadTheta = 0;
  510.                 for (int n = FastMath.max(2, m); n <= degree; ++n) {
  511.                     final double qSnm         = aOrN[n] * harmonics.getNormalizedSnm(n, m);
  512.                     final double qCnm         = aOrN[n] * harmonics.getNormalizedCnm(n, m);
  513.                     final double nOr          = n / r;
  514.                     final double nnP1Or2      = nOr * (n + 1) / r;
  515.                     final double s0           = pnm0[n] * qSnm;
  516.                     final double c0           = pnm0[n] * qCnm;
  517.                     final double s1           = pnm1[n] * qSnm;
  518.                     final double c1           = pnm1[n] * qCnm;
  519.                     final double s2           = pnm2[n] * qSnm;
  520.                     final double c2           = pnm2[n] * qCnm;
  521.                     sumDegreeS               += s0;
  522.                     sumDegreeC               += c0;
  523.                     dSumDegreeSdR            -= nOr * s0;
  524.                     dSumDegreeCdR            -= nOr * c0;
  525.                     dSumDegreeSdTheta        += s1;
  526.                     dSumDegreeCdTheta        += c1;
  527.                     d2SumDegreeSdRdR         += nnP1Or2 * s0;
  528.                     d2SumDegreeSdRdTheta     -= nOr * s1;
  529.                     d2SumDegreeSdThetadTheta += s2;
  530.                     d2SumDegreeCdRdR         += nnP1Or2 * c0;
  531.                     d2SumDegreeCdRdTheta     -= nOr * c1;
  532.                     d2SumDegreeCdThetadTheta += c2;
  533.                 }

  534.                 // contribution to outer summation over order
  535.                 final double sML = cosSinLambda[1][m];
  536.                 final double cML = cosSinLambda[0][m];
  537.                 value            = value         * u + sML * sumDegreeS + cML * sumDegreeC;
  538.                 gradient[0]      = gradient[0]   * u + sML * dSumDegreeSdR + cML * dSumDegreeCdR;
  539.                 gradient[1]      = gradient[1]   * u + m * (cML * sumDegreeS - sML * sumDegreeC);
  540.                 gradient[2]      = gradient[2]   * u + sML * dSumDegreeSdTheta + cML * dSumDegreeCdTheta;
  541.                 hessian[0][0]    = hessian[0][0] * u + sML * d2SumDegreeSdRdR + cML * d2SumDegreeCdRdR;
  542.                 hessian[1][0]    = hessian[1][0] * u + m * (cML * dSumDegreeSdR - sML * dSumDegreeCdR);
  543.                 hessian[2][0]    = hessian[2][0] * u + sML * d2SumDegreeSdRdTheta + cML * d2SumDegreeCdRdTheta;
  544.                 hessian[1][1]    = hessian[1][1] * u - m * m * (sML * sumDegreeS + cML * sumDegreeC);
  545.                 hessian[2][1]    = hessian[2][1] * u + m * (cML * dSumDegreeSdTheta - sML * dSumDegreeCdTheta);
  546.                 hessian[2][2]    = hessian[2][2] * u + sML * d2SumDegreeSdThetadTheta + cML * d2SumDegreeCdThetadTheta;

  547.             }

  548.             // rotate the recursion arrays
  549.             final double[] tmp0 = pnm0Plus2;
  550.             pnm0Plus2 = pnm0Plus1;
  551.             pnm0Plus1 = pnm0;
  552.             pnm0      = tmp0;
  553.             final double[] tmp1 = pnm1Plus1;
  554.             pnm1Plus1 = pnm1;
  555.             pnm1      = tmp1;

  556.         }

  557.         // scale back
  558.         value = FastMath.scalb(value, SCALING);
  559.         for (int i = 0; i < 3; ++i) {
  560.             gradient[i] = FastMath.scalb(gradient[i], SCALING);
  561.             for (int j = 0; j <= i; ++j) {
  562.                 hessian[i][j] = FastMath.scalb(hessian[i][j], SCALING);
  563.             }
  564.         }


  565.         // apply the global mu/r factor
  566.         final double muOr = mu / r;
  567.         value         *= muOr;
  568.         gradient[0]    = muOr * gradient[0] - value / r;
  569.         gradient[1]   *= muOr;
  570.         gradient[2]   *= muOr;
  571.         hessian[0][0]  = muOr * hessian[0][0] - 2 * gradient[0] / r;
  572.         hessian[1][0]  = muOr * hessian[1][0] -     gradient[1] / r;
  573.         hessian[2][0]  = muOr * hessian[2][0] -     gradient[2] / r;
  574.         hessian[1][1] *= muOr;
  575.         hessian[2][1] *= muOr;
  576.         hessian[2][2] *= muOr;

  577.         // convert gradient and Hessian from spherical to Cartesian
  578.         final SphericalCoordinates sc = new SphericalCoordinates(position);
  579.         return new GradientHessian(sc.toCartesianGradient(gradient),
  580.                                    sc.toCartesianHessian(hessian, gradient));


  581.     }

  582.     /** Container for gradient and Hessian. */
  583.     private static class GradientHessian {

  584.         /** Gradient. */
  585.         private final double[] gradient;

  586.         /** Hessian. */
  587.         private final double[][] hessian;

  588.         /** Simple constructor.
  589.          * <p>
  590.          * A reference to the arrays is stored, they are <strong>not</strong> cloned.
  591.          * </p>
  592.          * @param gradient gradient
  593.          * @param hessian hessian
  594.          */
  595.         GradientHessian(final double[] gradient, final double[][] hessian) {
  596.             this.gradient = gradient;
  597.             this.hessian  = hessian;
  598.         }

  599.         /** Get a reference to the gradient.
  600.          * @return gradient (a reference to the internal array is returned)
  601.          */
  602.         public double[] getGradient() {
  603.             return gradient;
  604.         }

  605.         /** Get a reference to the Hessian.
  606.          * @return Hessian (a reference to the internal array is returned)
  607.          */
  608.         public double[][] getHessian() {
  609.             return hessian;
  610.         }

  611.     }

  612.     /** Compute a/r powers array.
  613.      * @param aOr a/r
  614.      * @return array containing (a/r)<sup>n</sup>
  615.      */
  616.     private double[] createDistancePowersArray(final double aOr) {

  617.         // initialize array
  618.         final double[] aOrN = new double[provider.getMaxDegree() + 1];
  619.         aOrN[0] = 1;
  620.         if (provider.getMaxDegree() > 0) {
  621.             aOrN[1] = aOr;
  622.         }

  623.         // fill up array
  624.         for (int n = 2; n < aOrN.length; ++n) {
  625.             final int p = n / 2;
  626.             final int q = n - p;
  627.             aOrN[n] = aOrN[p] * aOrN[q];
  628.         }

  629.         return aOrN;

  630.     }
  631.     /** Compute a/r powers array.
  632.      * @param aOr a/r
  633.      * @param <T> type of field used
  634.      * @return array containing (a/r)<sup>n</sup>
  635.      */
  636.     private <T extends CalculusFieldElement<T>> T[] createDistancePowersArray(final T aOr) {

  637.         // initialize array
  638.         final T[] aOrN = MathArrays.buildArray(aOr.getField(), provider.getMaxDegree() + 1);
  639.         aOrN[0] = aOr.getField().getOne();
  640.         if (provider.getMaxDegree() > 0) {
  641.             aOrN[1] = aOr;
  642.         }

  643.         // fill up array
  644.         for (int n = 2; n < aOrN.length; ++n) {
  645.             final int p = n / 2;
  646.             final int q = n - p;
  647.             aOrN[n] = aOrN[p].multiply(aOrN[q]);
  648.         }

  649.         return aOrN;

  650.     }

  651.     /** Compute longitude cosines and sines.
  652.      * @param cosLambda cos(λ)
  653.      * @param sinLambda sin(λ)
  654.      * @return array containing cos(m &times; λ) in row 0
  655.      * and sin(m &times; λ) in row 1
  656.      */
  657.     private double[][] createCosSinArrays(final double cosLambda, final double sinLambda) {

  658.         // initialize arrays
  659.         final double[][] cosSin = new double[2][provider.getMaxOrder() + 1];
  660.         cosSin[0][0] = 1;
  661.         cosSin[1][0] = 0;
  662.         if (provider.getMaxOrder() > 0) {
  663.             cosSin[0][1] = cosLambda;
  664.             cosSin[1][1] = sinLambda;

  665.             // fill up array
  666.             for (int m = 2; m < cosSin[0].length; ++m) {

  667.                 // m * lambda is split as p * lambda + q * lambda, trying to avoid
  668.                 // p or q being much larger than the other. This reduces the number of
  669.                 // intermediate results reused to compute each value, and hence should limit
  670.                 // as much as possible roundoff error accumulation
  671.                 // (this does not change the number of floating point operations)
  672.                 final int p = m / 2;
  673.                 final int q = m - p;

  674.                 cosSin[0][m] = cosSin[0][p] * cosSin[0][q] - cosSin[1][p] * cosSin[1][q];
  675.                 cosSin[1][m] = cosSin[1][p] * cosSin[0][q] + cosSin[0][p] * cosSin[1][q];
  676.             }
  677.         }

  678.         return cosSin;

  679.     }

  680.     /** Compute longitude cosines and sines.
  681.      * @param cosLambda cos(λ)
  682.      * @param sinLambda sin(λ)
  683.      * @param <T> type of field used
  684.      * @return array containing cos(m &times; λ) in row 0
  685.      * and sin(m &times; λ) in row 1
  686.      */
  687.     private <T extends CalculusFieldElement<T>> T[][] createCosSinArrays(final T cosLambda, final T sinLambda) {

  688.         final T one = cosLambda.getField().getOne();
  689.         final T zero = cosLambda.getField().getZero();
  690.         // initialize arrays
  691.         final T[][] cosSin = MathArrays.buildArray(one.getField(), 2, provider.getMaxOrder() + 1);
  692.         cosSin[0][0] = one;
  693.         cosSin[1][0] = zero;
  694.         if (provider.getMaxOrder() > 0) {
  695.             cosSin[0][1] = cosLambda;
  696.             cosSin[1][1] = sinLambda;

  697.             // fill up array
  698.             for (int m = 2; m < cosSin[0].length; ++m) {

  699.                 // m * lambda is split as p * lambda + q * lambda, trying to avoid
  700.                 // p or q being much larger than the other. This reduces the number of
  701.                 // intermediate results reused to compute each value, and hence should limit
  702.                 // as much as possible roundoff error accumulation
  703.                 // (this does not change the number of floating point operations)
  704.                 final int p = m / 2;
  705.                 final int q = m - p;

  706.                 cosSin[0][m] = cosSin[0][p].multiply(cosSin[0][q]).subtract(cosSin[1][p].multiply(cosSin[1][q]));
  707.                 cosSin[1][m] = cosSin[1][p].multiply(cosSin[0][q]).add(cosSin[0][p].multiply(cosSin[1][q]));

  708.             }
  709.         }

  710.         return cosSin;

  711.     }

  712.     /** Compute one order of tesseral terms.
  713.      * <p>
  714.      * This corresponds to equations 27 and 30 of the paper.
  715.      * </p>
  716.      * @param m current order
  717.      * @param degree max degree
  718.      * @param index index in the flattened array
  719.      * @param t cos(θ), where θ is the polar angle
  720.      * @param u sin(θ), where θ is the polar angle
  721.      * @param tOu t/u
  722.      * @param pnm0Plus2 array containing scaled P<sub>n,m+2</sub>/u<sup>m+2</sup>
  723.      * @param pnm0Plus1 array containing scaled P<sub>n,m+1</sub>/u<sup>m+1</sup>
  724.      * @param pnm1Plus1 array containing scaled dP<sub>n,m+1</sub>/u<sup>m+1</sup>
  725.      * (may be null if second derivatives are not needed)
  726.      * @param pnm0 array to fill with scaled P<sub>n,m</sub>/u<sup>m</sup>
  727.      * @param pnm1 array to fill with scaled dP<sub>n,m</sub>/u<sup>m</sup>
  728.      * (may be null if first derivatives are not needed)
  729.      * @param pnm2 array to fill with scaled d²P<sub>n,m</sub>/u<sup>m</sup>
  730.      * (may be null if second derivatives are not needed)
  731.      * @return new value for index
  732.      */
  733.     private int computeTesseral(final int m, final int degree, final int index,
  734.                                 final double t, final double u, final double tOu,
  735.                                 final double[] pnm0Plus2, final double[] pnm0Plus1, final double[] pnm1Plus1,
  736.                                 final double[] pnm0, final double[] pnm1, final double[] pnm2) {

  737.         final double u2 = u * u;

  738.         // initialize recursion from sectorial terms
  739.         int n = FastMath.max(2, m);
  740.         if (n == m) {
  741.             pnm0[n] = sectorial[n];
  742.             ++n;
  743.         }

  744.         // compute tesseral values
  745.         int localIndex = index;
  746.         while (n <= degree) {

  747.             // value (equation 27 of the paper)
  748.             pnm0[n] = gnmOj[localIndex] * t * pnm0Plus1[n] - hnmOj[localIndex] * u2 * pnm0Plus2[n];

  749.             ++localIndex;
  750.             ++n;

  751.         }

  752.         if (pnm1 != null) {

  753.             // initialize recursion from sectorial terms
  754.             n = FastMath.max(2, m);
  755.             if (n == m) {
  756.                 pnm1[n] = m * tOu * pnm0[n];
  757.                 ++n;
  758.             }

  759.             // compute tesseral values and derivatives with respect to polar angle
  760.             localIndex = index;
  761.             while (n <= degree) {

  762.                 // first derivative (equation 30 of the paper)
  763.                 pnm1[n] = m * tOu * pnm0[n] - enm[localIndex] * u * pnm0Plus1[n];

  764.                 ++localIndex;
  765.                 ++n;

  766.             }

  767.             if (pnm2 != null) {

  768.                 // initialize recursion from sectorial terms
  769.                 n = FastMath.max(2, m);
  770.                 if (n == m) {
  771.                     pnm2[n] = m * (tOu * pnm1[n] - pnm0[n] / u2);
  772.                     ++n;
  773.                 }

  774.                 // compute tesseral values and derivatives with respect to polar angle
  775.                 localIndex = index;
  776.                 while (n <= degree) {

  777.                     // second derivative (differential of equation 30 with respect to theta)
  778.                     pnm2[n] = m * (tOu * pnm1[n] - pnm0[n] / u2) - enm[localIndex] * u * pnm1Plus1[n];

  779.                     ++localIndex;
  780.                     ++n;

  781.                 }

  782.             }

  783.         }

  784.         return localIndex;

  785.     }

  786.     /** Compute one order of tesseral terms.
  787.      * <p>
  788.      * This corresponds to equations 27 and 30 of the paper.
  789.      * </p>
  790.      * @param m current order
  791.      * @param degree max degree
  792.      * @param index index in the flattened array
  793.      * @param t cos(θ), where θ is the polar angle
  794.      * @param u sin(θ), where θ is the polar angle
  795.      * @param tOu t/u
  796.      * @param pnm0Plus2 array containing scaled P<sub>n,m+2</sub>/u<sup>m+2</sup>
  797.      * @param pnm0Plus1 array containing scaled P<sub>n,m+1</sub>/u<sup>m+1</sup>
  798.      * @param pnm1Plus1 array containing scaled dP<sub>n,m+1</sub>/u<sup>m+1</sup>
  799.      * (may be null if second derivatives are not needed)
  800.      * @param pnm0 array to fill with scaled P<sub>n,m</sub>/u<sup>m</sup>
  801.      * @param pnm1 array to fill with scaled dP<sub>n,m</sub>/u<sup>m</sup>
  802.      * (may be null if first derivatives are not needed)
  803.      * @param pnm2 array to fill with scaled d²P<sub>n,m</sub>/u<sup>m</sup>
  804.      * (may be null if second derivatives are not needed)
  805.      * @param <T> instance of field element
  806.      * @return new value for index
  807.      */
  808.     private <T extends CalculusFieldElement<T>> int computeTesseral(final int m, final int degree, final int index,
  809.                                                                 final T t, final T u, final T tOu,
  810.                                                                 final T[] pnm0Plus2, final T[] pnm0Plus1, final T[] pnm1Plus1,
  811.                                                                 final T[] pnm0, final T[] pnm1, final T[] pnm2) {

  812.         final T u2 = u.multiply(u);
  813.         final T zero = u.getField().getZero();
  814.         // initialize recursion from sectorial terms
  815.         int n = FastMath.max(2, m);
  816.         if (n == m) {
  817.             pnm0[n] = zero.add(sectorial[n]);
  818.             ++n;
  819.         }

  820.         // compute tesseral values
  821.         int localIndex = index;
  822.         while (n <= degree) {

  823.             // value (equation 27 of the paper)
  824.             pnm0[n] = t.multiply(gnmOj[localIndex]).multiply(pnm0Plus1[n]).subtract(u2.multiply(pnm0Plus2[n]).multiply(hnmOj[localIndex]));
  825.             ++localIndex;
  826.             ++n;

  827.         }
  828.         if (pnm1 != null) {

  829.             // initialize recursion from sectorial terms
  830.             n = FastMath.max(2, m);
  831.             if (n == m) {
  832.                 pnm1[n] = tOu.multiply(m).multiply(pnm0[n]);
  833.                 ++n;
  834.             }

  835.             // compute tesseral values and derivatives with respect to polar angle
  836.             localIndex = index;
  837.             while (n <= degree) {

  838.                 // first derivative (equation 30 of the paper)
  839.                 pnm1[n] = tOu.multiply(m).multiply(pnm0[n]).subtract(u.multiply(enm[localIndex]).multiply(pnm0Plus1[n]));

  840.                 ++localIndex;
  841.                 ++n;

  842.             }

  843.             if (pnm2 != null) {

  844.                 // initialize recursion from sectorial terms
  845.                 n = FastMath.max(2, m);
  846.                 if (n == m) {
  847.                     pnm2[n] =   tOu.multiply(pnm1[n]).subtract(pnm0[n].divide(u2)).multiply(m);
  848.                     ++n;
  849.                 }

  850.                 // compute tesseral values and derivatives with respect to polar angle
  851.                 localIndex = index;
  852.                 while (n <= degree) {

  853.                     // second derivative (differential of equation 30 with respect to theta)
  854.                     pnm2[n] = tOu.multiply(pnm1[n]).subtract(pnm0[n].divide(u2)).multiply(m).subtract(u.multiply(pnm1Plus1[n]).multiply(enm[localIndex]));
  855.                     ++localIndex;
  856.                     ++n;

  857.                 }

  858.             }

  859.         }
  860.         return localIndex;

  861.     }

  862.     /** {@inheritDoc} */
  863.     @Override
  864.     public Vector3D acceleration(final SpacecraftState s, final double[] parameters) {

  865.         final double mu = parameters[0];

  866.         // get the position in body frame
  867.         final AbsoluteDate date       = s.getDate();
  868.         final StaticTransform fromBodyFrame =
  869.                 bodyFrame.getStaticTransformTo(s.getFrame(), date);
  870.         final StaticTransform toBodyFrame   = fromBodyFrame.getInverse();
  871.         final Vector3D position       = toBodyFrame.transformPosition(s.getPVCoordinates().getPosition());

  872.         // gradient of the non-central part of the gravity field
  873.         return fromBodyFrame.transformVector(new Vector3D(gradient(date, position, mu)));

  874.     }

  875.     /** {@inheritDoc} */
  876.     public <T extends CalculusFieldElement<T>> FieldVector3D<T> acceleration(final FieldSpacecraftState<T> s,
  877.                                                                          final T[] parameters) {

  878.         final T mu = parameters[0];

  879.         // check for faster computation dedicated to derivatives with respect to state
  880.         if (isGradientStateDerivative(s)) {
  881.             @SuppressWarnings("unchecked")
  882.             final FieldVector3D<Gradient> p = (FieldVector3D<Gradient>) s.getPVCoordinates().getPosition();
  883.             @SuppressWarnings("unchecked")
  884.             final FieldVector3D<T> a = (FieldVector3D<T>) accelerationWrtState(s.getDate().toAbsoluteDate(),
  885.                                                                                s.getFrame(), p,
  886.                                                                                (Gradient) mu);
  887.             return a;
  888.         } else if (isDSStateDerivative(s)) {
  889.             @SuppressWarnings("unchecked")
  890.             final FieldVector3D<DerivativeStructure> p = (FieldVector3D<DerivativeStructure>) s.getPVCoordinates().getPosition();
  891.             @SuppressWarnings("unchecked")
  892.             final FieldVector3D<T> a = (FieldVector3D<T>) accelerationWrtState(s.getDate().toAbsoluteDate(),
  893.                                                                                s.getFrame(), p,
  894.                                                                                (DerivativeStructure) mu);
  895.             return a;
  896.         }

  897.         // get the position in body frame
  898.         final FieldAbsoluteDate<T> date          = s.getDate();
  899.         final StaticTransform      fromBodyFrame =
  900.                 bodyFrame.getStaticTransformTo(s.getFrame(), date.toAbsoluteDate());
  901.         final StaticTransform      toBodyFrame   = fromBodyFrame.getInverse();
  902.         final FieldVector3D<T>     position      = toBodyFrame.transformPosition(s.getPVCoordinates().getPosition());

  903.         // gradient of the non-central part of the gravity field
  904.         return fromBodyFrame.transformVector(new FieldVector3D<>(gradient(date, position, mu)));

  905.     }

  906.     /** {@inheritDoc} */
  907.     public Stream<EventDetector> getEventsDetectors() {
  908.         return Stream.empty();
  909.     }

  910.     @Override
  911.     /** {@inheritDoc} */
  912.     public <T extends CalculusFieldElement<T>> Stream<FieldEventDetector<T>> getFieldEventsDetectors(final Field<T> field) {
  913.         return Stream.empty();
  914.     }

  915.     /** Check if a field state corresponds to derivatives with respect to state.
  916.      * @param state state to check
  917.      * @param <T> type of the filed elements
  918.      * @return true if state corresponds to derivatives with respect to state
  919.      * @since 9.0
  920.      */
  921.     private <T extends CalculusFieldElement<T>> boolean isDSStateDerivative(final FieldSpacecraftState<T> state) {
  922.         try {
  923.             final DerivativeStructure dsMass = (DerivativeStructure) state.getMass();
  924.             final int o = dsMass.getOrder();
  925.             final int p = dsMass.getFreeParameters();
  926.             if (o != 1 || p < 3) {
  927.                 return false;
  928.             }
  929.             @SuppressWarnings("unchecked")
  930.             final FieldPVCoordinates<DerivativeStructure> pv = (FieldPVCoordinates<DerivativeStructure>) state.getPVCoordinates();
  931.             return isVariable(pv.getPosition().getX(), 0) &&
  932.                    isVariable(pv.getPosition().getY(), 1) &&
  933.                    isVariable(pv.getPosition().getZ(), 2);
  934.         } catch (ClassCastException cce) {
  935.             return false;
  936.         }
  937.     }

  938.     /** Check if a field state corresponds to derivatives with respect to state.
  939.      * @param state state to check
  940.      * @param <T> type of the filed elements
  941.      * @return true if state corresponds to derivatives with respect to state
  942.      * @since 10.2
  943.      */
  944.     private <T extends CalculusFieldElement<T>> boolean isGradientStateDerivative(final FieldSpacecraftState<T> state) {
  945.         try {
  946.             final Gradient gMass = (Gradient) state.getMass();
  947.             final int p = gMass.getFreeParameters();
  948.             if (p < 3) {
  949.                 return false;
  950.             }
  951.             @SuppressWarnings("unchecked")
  952.             final FieldPVCoordinates<Gradient> pv = (FieldPVCoordinates<Gradient>) state.getPVCoordinates();
  953.             return isVariable(pv.getPosition().getX(), 0) &&
  954.                    isVariable(pv.getPosition().getY(), 1) &&
  955.                    isVariable(pv.getPosition().getZ(), 2);
  956.         } catch (ClassCastException cce) {
  957.             return false;
  958.         }
  959.     }

  960.     /** Check if a derivative represents a specified variable.
  961.      * @param ds derivative to check
  962.      * @param index index of the variable
  963.      * @return true if the derivative represents a specified variable
  964.      * @since 9.0
  965.      */
  966.     private boolean isVariable(final DerivativeStructure ds, final int index) {
  967.         final double[] derivatives = ds.getAllDerivatives();
  968.         boolean check = true;
  969.         for (int i = 1; i < derivatives.length; ++i) {
  970.             check &= derivatives[i] == ((index + 1 == i) ? 1.0 : 0.0);
  971.         }
  972.         return check;
  973.     }

  974.     /** Check if a derivative represents a specified variable.
  975.      * @param g derivative to check
  976.      * @param index index of the variable
  977.      * @return true if the derivative represents a specified variable
  978.      * @since 10.2
  979.      */
  980.     private boolean isVariable(final Gradient g, final int index) {
  981.         final double[] derivatives = g.getGradient();
  982.         boolean check = true;
  983.         for (int i = 0; i < derivatives.length; ++i) {
  984.             check &= derivatives[i] == ((index == i) ? 1.0 : 0.0);
  985.         }
  986.         return check;
  987.     }

  988.     /** Compute acceleration derivatives with respect to state parameters.
  989.      * <p>
  990.      * From a theoretical point of view, this method computes the same values
  991.      * as {@link #acceleration(FieldSpacecraftState, CalculusFieldElement[])} in the
  992.      * specific case of {@link DerivativeStructure} with respect to state, so
  993.      * it is less general. However, it is *much* faster in this important case.
  994.      * <p>
  995.      * <p>
  996.      * The derivatives should be computed with respect to position. The input
  997.      * parameters already take into account the free parameters (6 or 7 depending
  998.      * on derivation with respect to mass being considered or not) and order
  999.      * (always 1). Free parameters at indices 0, 1 and 2 correspond to derivatives
  1000.      * with respect to position. Free parameters at indices 3, 4 and 5 correspond
  1001.      * to derivatives with respect to velocity (these derivatives will remain zero
  1002.      * as acceleration due to gravity does not depend on velocity). Free parameter
  1003.      * at index 6 (if present) corresponds to to derivatives with respect to mass
  1004.      * (this derivative will remain zero as acceleration due to gravity does not
  1005.      * depend on mass).
  1006.      * </p>
  1007.      * @param date current date
  1008.      * @param frame inertial reference frame for state (both orbit and attitude)
  1009.      * @param position position of spacecraft in inertial frame
  1010.      * @param mu central attraction coefficient to use
  1011.      * @return acceleration with all derivatives specified by the input parameters
  1012.      * own derivatives
  1013.      * @since 6.0
  1014.      */
  1015.     private FieldVector3D<DerivativeStructure> accelerationWrtState(final AbsoluteDate date, final Frame frame,
  1016.                                                                     final FieldVector3D<DerivativeStructure> position,
  1017.                                                                     final DerivativeStructure mu) {

  1018.         // free parameters
  1019.         final int freeParameters = mu.getFreeParameters();

  1020.         // get the position in body frame
  1021.         final StaticTransform fromBodyFrame =
  1022.                 bodyFrame.getStaticTransformTo(frame, date);
  1023.         final StaticTransform toBodyFrame   = fromBodyFrame.getInverse();
  1024.         final Vector3D positionBody   = toBodyFrame.transformPosition(position.toVector3D());

  1025.         // compute gradient and Hessian
  1026.         final GradientHessian gh   = gradientHessian(date, positionBody, mu.getReal());

  1027.         // gradient of the non-central part of the gravity field
  1028.         final double[] gInertial = fromBodyFrame.transformVector(new Vector3D(gh.getGradient())).toArray();

  1029.         // Hessian of the non-central part of the gravity field
  1030.         final RealMatrix hBody     = new Array2DRowRealMatrix(gh.getHessian(), false);
  1031.         final RealMatrix rot       = new Array2DRowRealMatrix(toBodyFrame.getRotation().getMatrix());
  1032.         final RealMatrix hInertial = rot.transpose().multiply(hBody).multiply(rot);

  1033.         // distribute all partial derivatives in a compact acceleration vector
  1034.         final double[] derivatives = new double[freeParameters + 1];
  1035.         final DerivativeStructure[] accDer = new DerivativeStructure[3];
  1036.         for (int i = 0; i < 3; ++i) {

  1037.             // first element is value of acceleration (i.e. gradient of field)
  1038.             derivatives[0] = gInertial[i];

  1039.             // Jacobian of acceleration (i.e. Hessian of field)
  1040.             derivatives[1] = hInertial.getEntry(i, 0);
  1041.             derivatives[2] = hInertial.getEntry(i, 1);
  1042.             derivatives[3] = hInertial.getEntry(i, 2);

  1043.             // next element is derivative with respect to parameter mu
  1044.             if (derivatives.length > 4 && isVariable(mu, 3)) {
  1045.                 derivatives[4] = gInertial[i] / mu.getReal();
  1046.             }

  1047.             accDer[i] = position.getX().getFactory().build(derivatives);

  1048.         }

  1049.         return new FieldVector3D<>(accDer);

  1050.     }

  1051.     /** Compute acceleration derivatives with respect to state parameters.
  1052.      * <p>
  1053.      * From a theoretical point of view, this method computes the same values
  1054.      * as {@link #acceleration(FieldSpacecraftState, CalculusFieldElement[])} in the
  1055.      * specific case of {@link DerivativeStructure} with respect to state, so
  1056.      * it is less general. However, it is *much* faster in this important case.
  1057.      * <p>
  1058.      * <p>
  1059.      * The derivatives should be computed with respect to position. The input
  1060.      * parameters already take into account the free parameters (6 or 7 depending
  1061.      * on derivation with respect to mass being considered or not) and order
  1062.      * (always 1). Free parameters at indices 0, 1 and 2 correspond to derivatives
  1063.      * with respect to position. Free parameters at indices 3, 4 and 5 correspond
  1064.      * to derivatives with respect to velocity (these derivatives will remain zero
  1065.      * as acceleration due to gravity does not depend on velocity). Free parameter
  1066.      * at index 6 (if present) corresponds to to derivatives with respect to mass
  1067.      * (this derivative will remain zero as acceleration due to gravity does not
  1068.      * depend on mass).
  1069.      * </p>
  1070.      * @param date current date
  1071.      * @param frame inertial reference frame for state (both orbit and attitude)
  1072.      * @param position position of spacecraft in inertial frame
  1073.      * @param mu central attraction coefficient to use
  1074.      * @return acceleration with all derivatives specified by the input parameters
  1075.      * own derivatives
  1076.      * @since 10.2
  1077.      */
  1078.     private FieldVector3D<Gradient> accelerationWrtState(final AbsoluteDate date, final Frame frame,
  1079.                                                          final FieldVector3D<Gradient> position,
  1080.                                                          final Gradient mu) {

  1081.         // free parameters
  1082.         final int freeParameters = mu.getFreeParameters();

  1083.         // get the position in body frame
  1084.         final StaticTransform fromBodyFrame = bodyFrame.getStaticTransformTo(frame, date);
  1085.         final StaticTransform toBodyFrame   = fromBodyFrame.getInverse();
  1086.         final Vector3D positionBody   = toBodyFrame.transformPosition(position.toVector3D());

  1087.         // compute gradient and Hessian
  1088.         final GradientHessian gh   = gradientHessian(date, positionBody, mu.getReal());

  1089.         // gradient of the non-central part of the gravity field
  1090.         final double[] gInertial = fromBodyFrame.transformVector(new Vector3D(gh.getGradient())).toArray();

  1091.         // Hessian of the non-central part of the gravity field
  1092.         final RealMatrix hBody     = new Array2DRowRealMatrix(gh.getHessian(), false);
  1093.         final RealMatrix rot       = new Array2DRowRealMatrix(toBodyFrame.getRotation().getMatrix());
  1094.         final RealMatrix hInertial = rot.transpose().multiply(hBody).multiply(rot);

  1095.         // distribute all partial derivatives in a compact acceleration vector
  1096.         final double[] derivatives = new double[freeParameters];
  1097.         final Gradient[] accDer = new Gradient[3];
  1098.         for (int i = 0; i < 3; ++i) {

  1099.             // Jacobian of acceleration (i.e. Hessian of field)
  1100.             derivatives[0] = hInertial.getEntry(i, 0);
  1101.             derivatives[1] = hInertial.getEntry(i, 1);
  1102.             derivatives[2] = hInertial.getEntry(i, 2);

  1103.             // next element is derivative with respect to parameter mu
  1104.             if (derivatives.length > 3 && isVariable(mu, 3)) {
  1105.                 derivatives[3] = gInertial[i] / mu.getReal();
  1106.             }

  1107.             accDer[i] = new Gradient(gInertial[i], derivatives);

  1108.         }

  1109.         return new FieldVector3D<>(accDer);

  1110.     }

  1111.     /** {@inheritDoc} */
  1112.     public List<ParameterDriver> getParametersDrivers() {
  1113.         return Collections.singletonList(gmParameterDriver);
  1114.     }

  1115. }