HolmesFeatherstoneAttractionModel.java

  1. /* Copyright 2002-2024 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;


  20. import java.util.Collections;
  21. import java.util.List;

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

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

  75. public class HolmesFeatherstoneAttractionModel implements ForceModel, TideSystemProvider {

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

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

  91.     /** Driver for gravitational parameter. */
  92.     private final ParameterDriver gmParameterDriver;

  94.     /** Provider for the spherical harmonics. */
  95.     private final NormalizedSphericalHarmonicsProvider provider;

  97.     /** Rotating body. */
  98.     private final Frame bodyFrame;

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

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

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

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

  112.     /** Creates a new instance.
  113.      * @param centralBodyFrame rotating body frame
  114.      * @param provider provider for spherical harmonics
  115.      * @since 6.0
  116.      */
  117.     public HolmesFeatherstoneAttractionModel(final Frame centralBodyFrame,
  118.                                              final NormalizedSphericalHarmonicsProvider provider) {

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

  123.         this.provider  = provider;
  124.         this.bodyFrame = centralBodyFrame;

  126.         // the pre-computed arrays hold coefficients from triangular arrays in a single
  127.         // storing neither diagonal elements (n = m) nor the non-diagonal element n=1, m=0
  128.         final int degree = provider.getMaxDegree();
  129.         final int size = FastMath.max(0, degree * (degree + 1) / 2 - 1);
  130.         gnmOj = new double[size];
  131.         hnmOj = new double[size];
  132.         enm   = new double[size];

  134.         // pre-compute the recursion coefficients corresponding to equations 19 and 22
  135.         // from Holmes and Featherstone paper
  136.         // for cache efficiency, elements are stored in the same order they will be used
  137.         // later on, i.e. from rightmost column to leftmost column
  138.         int index = 0;
  139.         for (int m = degree; m >= 0; --m) {
  140.             final int j = (m == 0) ? 2 : 1;
  141.             for (int n = FastMath.max(2, m + 1); n <= degree; ++n) {
  142.                 final double f = (n - m) * (n + m + 1);
  143.                 gnmOj[index] = 2 * (m + 1) / FastMath.sqrt(j * f);
  144.                 hnmOj[index] = FastMath.sqrt((n + m + 2) * (n - m - 1) / (j * f));
  145.                 enm[index]   = FastMath.sqrt(f / j);
  146.                 ++index;
  147.             }
  148.         }

  150.         // scaled sectorial terms corresponding to equation 28 in Holmes and Featherstone paper
  151.         sectorial    = new double[degree + 1];
  152.         sectorial[0] = FastMath.scalb(1.0, -SCALING);
  153.         if (degree > 0) {
  154.             sectorial[1] = FastMath.sqrt(3) * sectorial[0];
  155.         }
  156.         for (int m = 2; m < sectorial.length; ++m) {
  157.             sectorial[m] = FastMath.sqrt((2 * m + 1) / (2.0 * m)) * sectorial[m - 1];
  158.         }

  160.     }

  162.     /** {@inheritDoc} */
  163.     @Override
  164.     public boolean dependsOnPositionOnly() {
  165.         return true;
  166.     }

  168.     /** {@inheritDoc} */
  169.     public TideSystem getTideSystem() {
  170.         return provider.getTideSystem();
  171.     }

  173.     /** Get the central attraction coefficient μ.
  174.      * @return mu central attraction coefficient (m³/s²),
  175.      * will throw an exception if gm PDriver has several
  176.      * values driven (in this case the method
  177.      * {@link #getMu(AbsoluteDate)} must be used.
  178.      */
  179.     public double getMu() {
  180.         return gmParameterDriver.getValue();
  181.     }

  183.     /** Get the central attraction coefficient μ.
  184.      * @param date date at which mu wants to be known
  185.      * @return mu central attraction coefficient (m³/s²)
  186.      */
  187.     public double getMu(final AbsoluteDate date) {
  188.         return gmParameterDriver.getValue(date);
  189.     }

  191.     /** Compute the value of the gravity field.
  192.      * @param date current date
  193.      * @param position position at which gravity field is desired in body frame
  194.      * @param mu central attraction coefficient to use
  195.      * @return value of the gravity field (central and non-central parts summed together)
  196.      */
  197.     public double value(final AbsoluteDate date, final Vector3D position,
  198.                         final double mu) {
  199.         return mu / position.getNorm() + nonCentralPart(date, position, mu);
  200.     }

  202.     /** Compute the non-central part of the gravity field.
  203.      * @param date current date
  204.      * @param position position at which gravity field is desired in body frame
  205.      * @param mu central attraction coefficient to use
  206.      * @return value of the non-central part of the gravity field
  207.      */
  208.     public double nonCentralPart(final AbsoluteDate date, final Vector3D position, final double mu) {

  210.         final int degree = provider.getMaxDegree();
  211.         final int order  = provider.getMaxOrder();
  212.         final NormalizedSphericalHarmonics harmonics = provider.onDate(date);

  214.         // allocate the columns for recursion
  215.         double[] pnm0Plus2 = new double[degree + 1];
  216.         double[] pnm0Plus1 = new double[degree + 1];
  217.         double[] pnm0      = new double[degree + 1];

  219.         // compute polar coordinates
  220.         final double x    = position.getX();
  221.         final double y    = position.getY();
  222.         final double z    = position.getZ();
  223.         final double x2   = x * x;
  224.         final double y2   = y * y;
  225.         final double z2   = z * z;
  226.         final double rho2 = x2 + y2;
  227.         final double r2   = rho2 + z2;
  228.         final double r    = FastMath.sqrt(r2);
  229.         final double rho  = FastMath.sqrt(rho2);
  230.         final double t    = z / r;   // cos(theta), where theta is the polar angle
  231.         final double u    = rho / r; // sin(theta), where theta is the polar angle
  232.         final double tOu  = z / rho;

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

  237.         // compute longitude cosines/sines
  238.         final double[][] cosSinLambda = createCosSinArrays(x / rho, y / rho);

  240.         // outer summation over order
  241.         int    index = 0;
  242.         double value = 0;
  243.         for (int m = degree; m >= 0; --m) {

  245.             // compute tesseral terms without derivatives
  246.             index = computeTesseral(m, degree, index, t, u, tOu,
  247.                                     pnm0Plus2, pnm0Plus1, null, pnm0, null, null);

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

  252.                 // inner summation over degree, for fixed order
  253.                 double sumDegreeS        = 0;
  254.                 double sumDegreeC        = 0;
  255.                 for (int n = FastMath.max(2, m); n <= degree; ++n) {
  256.                     sumDegreeS += pnm0[n] * aOrN[n] * harmonics.getNormalizedSnm(n, m);
  257.                     sumDegreeC += pnm0[n] * aOrN[n] * harmonics.getNormalizedCnm(n, m);
  258.                 }

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

  263.             }

  265.             // rotate the recursion arrays
  266.             final double[] tmp = pnm0Plus2;
  267.             pnm0Plus2 = pnm0Plus1;
  268.             pnm0Plus1 = pnm0;
  269.             pnm0      = tmp;

  271.         }

  273.         // scale back
  274.         value = FastMath.scalb(value, SCALING);

  276.         // apply the global mu/r factor
  277.         return mu * value / r;

  279.     }

  281.     /** Compute the gradient of the non-central part of the gravity field.
  282.      * @param date current date
  283.      * @param position position at which gravity field is desired in body frame
  284.      * @param mu central attraction coefficient to use
  285.      * @return gradient of the non-central part of the gravity field
  286.      */
  287.     public double[] gradient(final AbsoluteDate date, final Vector3D position, final double mu) {

  289.         final int degree = provider.getMaxDegree();
  290.         final int order  = provider.getMaxOrder();
  291.         final NormalizedSphericalHarmonics harmonics = provider.onDate(date);

  293.         // allocate the columns for recursion
  294.         double[] pnm0Plus2  = new double[degree + 1];
  295.         double[] pnm0Plus1  = new double[degree + 1];
  296.         double[] pnm0       = new double[degree + 1];
  297.         final double[] pnm1 = new double[degree + 1];

  299.         // compute polar coordinates
  300.         final double x    = position.getX();
  301.         final double y    = position.getY();
  302.         final double z    = position.getZ();
  303.         final double x2   = x * x;
  304.         final double y2   = y * y;
  305.         final double z2   = z * z;
  306.         final double r2   = x2 + y2 + z2;
  307.         final double r    = FastMath.sqrt (r2);
  308.         final double rho2 = x2 + y2;
  309.         final double rho  = FastMath.sqrt(rho2);
  310.         final double t    = z / r;   // cos(theta), where theta is the polar angle
  311.         final double u    = rho / r; // sin(theta), where theta is the polar angle
  312.         final double tOu  = z / rho;

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

  317.         // compute longitude cosines/sines
  318.         final double[][] cosSinLambda = createCosSinArrays(x / rho, y / rho);

  320.         // outer summation over order
  321.         int    index = 0;
  322.         double value = 0;
  323.         final double[] gradient = new double[3];
  324.         for (int m = degree; m >= 0; --m) {

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

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

  333.                 // inner summation over degree, for fixed order
  334.                 double sumDegreeS        = 0;
  335.                 double sumDegreeC        = 0;
  336.                 double dSumDegreeSdR     = 0;
  337.                 double dSumDegreeCdR     = 0;
  338.                 double dSumDegreeSdTheta = 0;
  339.                 double dSumDegreeCdTheta = 0;
  340.                 for (int n = FastMath.max(2, m); n <= degree; ++n) {
  341.                     final double qSnm  = aOrN[n] * harmonics.getNormalizedSnm(n, m);
  342.                     final double qCnm  = aOrN[n] * harmonics.getNormalizedCnm(n, m);
  343.                     final double nOr   = n / r;
  344.                     final double s0    = pnm0[n] * qSnm;
  345.                     final double c0    = pnm0[n] * qCnm;
  346.                     final double s1    = pnm1[n] * qSnm;
  347.                     final double c1    = pnm1[n] * qCnm;
  348.                     sumDegreeS        += s0;
  349.                     sumDegreeC        += c0;
  350.                     dSumDegreeSdR     -= nOr * s0;
  351.                     dSumDegreeCdR     -= nOr * c0;
  352.                     dSumDegreeSdTheta += s1;
  353.                     dSumDegreeCdTheta += c1;
  354.                 }

  356.                 // contribution to outer summation over order
  357.                 // beware that we need to order gradient using the mathematical conventions
  358.                 // compliant with the SphericalCoordinates class, so our lambda is its theta
  359.                 // (and hence at index 1) and our theta is its phi (and hence at index 2)
  360.                 final double sML = cosSinLambda[1][m];
  361.                 final double cML = cosSinLambda[0][m];
  362.                 value            = value       * u + sML * sumDegreeS        + cML * sumDegreeC;
  363.                 gradient[0]      = gradient[0] * u + sML * dSumDegreeSdR     + cML * dSumDegreeCdR;
  364.                 gradient[1]      = gradient[1] * u + m * (cML * sumDegreeS - sML * sumDegreeC);
  365.                 gradient[2]      = gradient[2] * u + sML * dSumDegreeSdTheta + cML * dSumDegreeCdTheta;

  367.             }

  369.             // rotate the recursion arrays
  370.             final double[] tmp = pnm0Plus2;
  371.             pnm0Plus2 = pnm0Plus1;
  372.             pnm0Plus1 = pnm0;
  373.             pnm0      = tmp;

  375.         }

  377.         // scale back
  378.         value       = FastMath.scalb(value,       SCALING);
  379.         gradient[0] = FastMath.scalb(gradient[0], SCALING);
  380.         gradient[1] = FastMath.scalb(gradient[1], SCALING);
  381.         gradient[2] = FastMath.scalb(gradient[2], SCALING);

  383.         // apply the global mu/r factor
  384.         final double muOr = mu / r;
  385.         value            *= muOr;
  386.         gradient[0]       = muOr * gradient[0] - value / r;
  387.         gradient[1]      *= muOr;
  388.         gradient[2]      *= muOr;

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

  393.     }

  395.     /** Compute the gradient of the non-central part of the gravity field.
  396.      * @param date current date
  397.      * @param position position at which gravity field is desired in body frame
  398.      * @param mu central attraction coefficient to use
  399.      * @param <T> type of field used
  400.      * @return gradient of the non-central part of the gravity field
  401.      */
  402.     public <T extends CalculusFieldElement<T>> T[] gradient(final FieldAbsoluteDate<T> date, final FieldVector3D<T> position,
  403.                                                         final T mu) {

  405.         final int degree = provider.getMaxDegree();
  406.         final int order  = provider.getMaxOrder();
  407.         final NormalizedSphericalHarmonics harmonics = provider.onDate(date.toAbsoluteDate());
  408.         final T zero = date.getField().getZero();
  409.         // allocate the columns for recursion
  410.         T[] pnm0Plus2  = MathArrays.buildArray(date.getField(), degree + 1);
  411.         T[] pnm0Plus1  = MathArrays.buildArray(date.getField(), degree + 1);
  412.         T[] pnm0       = MathArrays.buildArray(date.getField(), degree + 1);
  413.         final T[] pnm1 = MathArrays.buildArray(date.getField(), degree + 1);

  415.         // compute polar coordinates
  416.         final T x    = position.getX();
  417.         final T y    = position.getY();
  418.         final T z    = position.getZ();
  419.         final T x2   = x.square();
  420.         final T y2   = y.square();
  421.         final T rho2 = x2.add(y2);
  422.         final T rho  = rho2.sqrt();
  423.         final T z2   = z.square();
  424.         final T r2   = rho2.add(z2);
  425.         final T r    = r2.sqrt();
  426.         final T t    = z.divide(r);   // cos(theta), where theta is the polar angle
  427.         final T u    = rho.divide(r); // sin(theta), where theta is the polar angle
  428.         final T tOu  = z.divide(rho);

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

  433.         // compute longitude cosines/sines
  434.         final T[][] cosSinLambda = createCosSinArrays(x.divide(rho), y.divide(rho));
  435.         // outer summation over order
  436.         int    index = 0;
  437.         T value = zero;
  438.         final T[] gradient = MathArrays.buildArray(zero.getField(), 3);
  439.         for (int m = degree; m >= 0; --m) {

  441.             // compute tesseral terms with derivatives
  442.             index = computeTesseral(m, degree, index, t, u, tOu,
  443.                                     pnm0Plus2, pnm0Plus1, null, pnm0, pnm1, null);
  444.             if (m <= order) {
  445.                 // compute contribution of current order to field (equation 5 of the paper)

  447.                 // inner summation over degree, for fixed order
  448.                 T sumDegreeS        = zero;
  449.                 T sumDegreeC        = zero;
  450.                 T dSumDegreeSdR     = zero;
  451.                 T dSumDegreeCdR     = zero;
  452.                 T dSumDegreeSdTheta = zero;
  453.                 T dSumDegreeCdTheta = zero;
  454.                 for (int n = FastMath.max(2, m); n <= degree; ++n) {
  455.                     final T qSnm  = aOrN[n].multiply(harmonics.getNormalizedSnm(n, m));
  456.                     final T qCnm  = aOrN[n].multiply(harmonics.getNormalizedCnm(n, m));
  457.                     final T nOr   = r.reciprocal().multiply(n);
  458.                     final T s0    = pnm0[n].multiply(qSnm);
  459.                     final T c0    = pnm0[n].multiply(qCnm);
  460.                     final T s1    = pnm1[n].multiply(qSnm);
  461.                     final T c1    = pnm1[n].multiply(qCnm);
  462.                     sumDegreeS        = sumDegreeS       .add(s0);
  463.                     sumDegreeC        = sumDegreeC       .add(c0);
  464.                     dSumDegreeSdR     = dSumDegreeSdR    .subtract(nOr.multiply(s0));
  465.                     dSumDegreeCdR     = dSumDegreeCdR    .subtract(nOr.multiply(c0));
  466.                     dSumDegreeSdTheta = dSumDegreeSdTheta.add(s1);
  467.                     dSumDegreeCdTheta = dSumDegreeCdTheta.add(c1);
  468.                 }

  470.                 // contribution to outer summation over order
  471.                 // beware that we need to order gradient using the mathematical conventions
  472.                 // compliant with the SphericalCoordinates class, so our lambda is its theta
  473.                 // (and hence at index 1) and our theta is its phi (and hence at index 2)
  474.                 final T sML = cosSinLambda[1][m];
  475.                 final T cML = cosSinLambda[0][m];
  476.                 value            = value      .multiply(u).add(sML.multiply(sumDegreeS   )).add(cML.multiply(sumDegreeC));
  477.                 gradient[0]      = gradient[0].multiply(u).add(sML.multiply(dSumDegreeSdR)).add(cML.multiply(dSumDegreeCdR));
  478.                 gradient[1]      = gradient[1].multiply(u).add(cML.multiply(sumDegreeS).subtract(sML.multiply(sumDegreeC)).multiply(m));
  479.                 gradient[2]      = gradient[2].multiply(u).add(sML.multiply(dSumDegreeSdTheta)).add(cML.multiply(dSumDegreeCdTheta));
  480.             }
  481.             // rotate the recursion arrays
  482.             final T[] tmp = pnm0Plus2;
  483.             pnm0Plus2 = pnm0Plus1;
  484.             pnm0Plus1 = pnm0;
  485.             pnm0      = tmp;

  487.         }
  488.         // scale back
  489.         value       = value.scalb(SCALING);
  490.         gradient[0] = gradient[0].scalb(SCALING);
  491.         gradient[1] = gradient[1].scalb(SCALING);
  492.         gradient[2] = gradient[2].scalb(SCALING);

  494.         // apply the global mu/r factor
  495.         final T muOr = r.reciprocal().multiply(mu);
  496.         value            = value.multiply(muOr);
  497.         gradient[0]      = muOr.multiply(gradient[0]).subtract(value.divide(r));
  498.         gradient[1]      = gradient[1].multiply(muOr);
  499.         gradient[2]      = gradient[2].multiply(muOr);

  501.         // convert gradient from spherical to Cartesian
  502.         // Cartesian coordinates
  503.         // remaining spherical coordinates

  505.         // intermediate variables
  506.         final T xPos    = position.getX();
  507.         final T yPos    = position.getY();
  508.         final T zPos    = position.getZ();
  509.         final T rho2Pos = x.square().add(y.square());
  510.         final T rhoPos  = rho2.sqrt();
  511.         final T r2Pos   = rho2.add(z.square());
  512.         final T rPos    = r2Pos.sqrt();

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

  516.         // row representing the gradient of r
  517.         jacobianPos[0][0] = xPos.divide(rPos);
  518.         jacobianPos[0][1] = yPos.divide(rPos);
  519.         jacobianPos[0][2] = zPos.divide(rPos);

  521.         // row representing the gradient of theta
  522.         jacobianPos[1][0] =  yPos.negate().divide(rho2Pos);
  523.         jacobianPos[1][1] =  xPos.divide(rho2Pos);
  524.         // jacobian[1][2] is already set to 0 at allocation time

  526.         // row representing the gradient of phi
  527.         final T rhoPosTimesR2Pos = rhoPos.multiply(r2Pos);
  528.         jacobianPos[2][0] = xPos.multiply(zPos).divide(rhoPosTimesR2Pos);
  529.         jacobianPos[2][1] = yPos.multiply(zPos).divide(rhoPosTimesR2Pos);
  530.         jacobianPos[2][2] = rhoPos.negate().divide(r2Pos);
  531.         final T[] cartGradPos = MathArrays.buildArray(zero.getField(), 3);
  532.         cartGradPos[0] = gradient[0].multiply(jacobianPos[0][0]).add(gradient[1].multiply(jacobianPos[1][0])).add(gradient[2].multiply(jacobianPos[2][0]));
  533.         cartGradPos[1] = gradient[0].multiply(jacobianPos[0][1]).add(gradient[1].multiply(jacobianPos[1][1])).add(gradient[2].multiply(jacobianPos[2][1]));
  534.         cartGradPos[2] = gradient[0].multiply(jacobianPos[0][2])                                      .add(gradient[2].multiply(jacobianPos[2][2]));
  535.         return cartGradPos;

  537.     }

  539.     /** Compute both the gradient and the hessian of the non-central part of the gravity field.
  540.      * @param date current date
  541.      * @param position position at which gravity field is desired in body frame
  542.      * @param mu central attraction coefficient to use
  543.      * @return gradient and hessian of the non-central part of the gravity field
  544.      */
  545.     private GradientHessian gradientHessian(final AbsoluteDate date, final Vector3D position, final double mu) {

  547.         final int degree = provider.getMaxDegree();
  548.         final int order  = provider.getMaxOrder();
  549.         final NormalizedSphericalHarmonics harmonics = provider.onDate(date);

  551.         // allocate the columns for recursion
  552.         double[] pnm0Plus2  = new double[degree + 1];
  553.         double[] pnm0Plus1  = new double[degree + 1];
  554.         double[] pnm0       = new double[degree + 1];
  555.         double[] pnm1Plus1  = new double[degree + 1];
  556.         double[] pnm1       = new double[degree + 1];
  557.         final double[] pnm2 = new double[degree + 1];

  559.         // compute polar coordinates
  560.         final double x    = position.getX();
  561.         final double y    = position.getY();
  562.         final double z    = position.getZ();
  563.         final double x2   = x * x;
  564.         final double y2   = y * y;
  565.         final double z2   = z * z;
  566.         final double rho2 = x2 + y2;
  567.         final double rho  = FastMath.sqrt(rho2);
  568.         final double r2   = rho2 + z2;
  569.         final double r    = FastMath.sqrt(r2);
  570.         final double t    = z / r;   // cos(theta), where theta is the polar angle
  571.         final double u    = rho / r; // sin(theta), where theta is the polar angle
  572.         final double tOu  = z / rho;

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

  577.         // compute longitude cosines/sines
  578.         final double[][] cosSinLambda = createCosSinArrays(x / rho, y / rho);

  580.         // outer summation over order
  581.         int    index = 0;
  582.         double value = 0;
  583.         final double[]   gradient = new double[3];
  584.         final double[][] hessian  = new double[3][3];
  585.         for (int m = degree; m >= 0; --m) {

  587.             // compute tesseral terms
  588.             index = computeTesseral(m, degree, index, t, u, tOu,
  589.                                     pnm0Plus2, pnm0Plus1, pnm1Plus1, pnm0, pnm1, pnm2);

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

  594.                 // inner summation over degree, for fixed order
  595.                 double sumDegreeS               = 0;
  596.                 double sumDegreeC               = 0;
  597.                 double dSumDegreeSdR            = 0;
  598.                 double dSumDegreeCdR            = 0;
  599.                 double dSumDegreeSdTheta        = 0;
  600.                 double dSumDegreeCdTheta        = 0;
  601.                 double d2SumDegreeSdRdR         = 0;
  602.                 double d2SumDegreeSdRdTheta     = 0;
  603.                 double d2SumDegreeSdThetadTheta = 0;
  604.                 double d2SumDegreeCdRdR         = 0;
  605.                 double d2SumDegreeCdRdTheta     = 0;
  606.                 double d2SumDegreeCdThetadTheta = 0;
  607.                 for (int n = FastMath.max(2, m); n <= degree; ++n) {
  608.                     final double qSnm         = aOrN[n] * harmonics.getNormalizedSnm(n, m);
  609.                     final double qCnm         = aOrN[n] * harmonics.getNormalizedCnm(n, m);
  610.                     final double nOr          = n / r;
  611.                     final double nnP1Or2      = nOr * (n + 1) / r;
  612.                     final double s0           = pnm0[n] * qSnm;
  613.                     final double c0           = pnm0[n] * qCnm;
  614.                     final double s1           = pnm1[n] * qSnm;
  615.                     final double c1           = pnm1[n] * qCnm;
  616.                     final double s2           = pnm2[n] * qSnm;
  617.                     final double c2           = pnm2[n] * qCnm;
  618.                     sumDegreeS               += s0;
  619.                     sumDegreeC               += c0;
  620.                     dSumDegreeSdR            -= nOr * s0;
  621.                     dSumDegreeCdR            -= nOr * c0;
  622.                     dSumDegreeSdTheta        += s1;
  623.                     dSumDegreeCdTheta        += c1;
  624.                     d2SumDegreeSdRdR         += nnP1Or2 * s0;
  625.                     d2SumDegreeSdRdTheta     -= nOr * s1;
  626.                     d2SumDegreeSdThetadTheta += s2;
  627.                     d2SumDegreeCdRdR         += nnP1Or2 * c0;
  628.                     d2SumDegreeCdRdTheta     -= nOr * c1;
  629.                     d2SumDegreeCdThetadTheta += c2;
  630.                 }

  632.                 // contribution to outer summation over order
  633.                 final double sML = cosSinLambda[1][m];
  634.                 final double cML = cosSinLambda[0][m];
  635.                 value            = value         * u + sML * sumDegreeS + cML * sumDegreeC;
  636.                 gradient[0]      = gradient[0]   * u + sML * dSumDegreeSdR + cML * dSumDegreeCdR;
  637.                 gradient[1]      = gradient[1]   * u + m * (cML * sumDegreeS - sML * sumDegreeC);
  638.                 gradient[2]      = gradient[2]   * u + sML * dSumDegreeSdTheta + cML * dSumDegreeCdTheta;
  639.                 hessian[0][0]    = hessian[0][0] * u + sML * d2SumDegreeSdRdR + cML * d2SumDegreeCdRdR;
  640.                 hessian[1][0]    = hessian[1][0] * u + m * (cML * dSumDegreeSdR - sML * dSumDegreeCdR);
  641.                 hessian[2][0]    = hessian[2][0] * u + sML * d2SumDegreeSdRdTheta + cML * d2SumDegreeCdRdTheta;
  642.                 hessian[1][1]    = hessian[1][1] * u - m * m * (sML * sumDegreeS + cML * sumDegreeC);
  643.                 hessian[2][1]    = hessian[2][1] * u + m * (cML * dSumDegreeSdTheta - sML * dSumDegreeCdTheta);
  644.                 hessian[2][2]    = hessian[2][2] * u + sML * d2SumDegreeSdThetadTheta + cML * d2SumDegreeCdThetadTheta;

  646.             }

  648.             // rotate the recursion arrays
  649.             final double[] tmp0 = pnm0Plus2;
  650.             pnm0Plus2 = pnm0Plus1;
  651.             pnm0Plus1 = pnm0;
  652.             pnm0      = tmp0;
  653.             final double[] tmp1 = pnm1Plus1;
  654.             pnm1Plus1 = pnm1;
  655.             pnm1      = tmp1;

  657.         }

  659.         // scale back
  660.         value = FastMath.scalb(value, SCALING);
  661.         for (int i = 0; i < 3; ++i) {
  662.             gradient[i] = FastMath.scalb(gradient[i], SCALING);
  663.             for (int j = 0; j <= i; ++j) {
  664.                 hessian[i][j] = FastMath.scalb(hessian[i][j], SCALING);
  665.             }
  666.         }


  669.         // apply the global mu/r factor
  670.         final double muOr = mu / r;
  671.         value         *= muOr;
  672.         gradient[0]    = muOr * gradient[0] - value / r;
  673.         gradient[1]   *= muOr;
  674.         gradient[2]   *= muOr;
  675.         hessian[0][0]  = muOr * hessian[0][0] - 2 * gradient[0] / r;
  676.         hessian[1][0]  = muOr * hessian[1][0] -     gradient[1] / r;
  677.         hessian[2][0]  = muOr * hessian[2][0] -     gradient[2] / r;
  678.         hessian[1][1] *= muOr;
  679.         hessian[2][1] *= muOr;
  680.         hessian[2][2] *= muOr;

  682.         // convert gradient and Hessian from spherical to Cartesian
  683.         final SphericalCoordinates sc = new SphericalCoordinates(position);
  684.         return new GradientHessian(sc.toCartesianGradient(gradient),
  685.                                    sc.toCartesianHessian(hessian, gradient));


  688.     }

  690.     /** Container for gradient and Hessian. */
  691.     private static class GradientHessian {

  693.         /** Gradient. */
  694.         private final double[] gradient;

  696.         /** Hessian. */
  697.         private final double[][] hessian;

  699.         /** Simple constructor.
  700.          * <p>
  701.          * A reference to the arrays is stored, they are <strong>not</strong> cloned.
  702.          * </p>
  703.          * @param gradient gradient
  704.          * @param hessian hessian
  705.          */
  706.         GradientHessian(final double[] gradient, final double[][] hessian) {
  707.             this.gradient = gradient;
  708.             this.hessian  = hessian;
  709.         }

  711.         /** Get a reference to the gradient.
  712.          * @return gradient (a reference to the internal array is returned)
  713.          */
  714.         public double[] getGradient() {
  715.             return gradient;
  716.         }

  718.         /** Get a reference to the Hessian.
  719.          * @return Hessian (a reference to the internal array is returned)
  720.          */
  721.         public double[][] getHessian() {
  722.             return hessian;
  723.         }

  725.     }

  727.     /** Compute a/r powers array.
  728.      * @param aOr a/r
  729.      * @return array containing (a/r)<sup>n</sup>
  730.      */
  731.     private double[] createDistancePowersArray(final double aOr) {

  733.         // initialize array
  734.         final double[] aOrN = new double[provider.getMaxDegree() + 1];
  735.         aOrN[0] = 1;
  736.         if (provider.getMaxDegree() > 0) {
  737.             aOrN[1] = aOr;
  738.         }

  740.         // fill up array
  741.         for (int n = 2; n < aOrN.length; ++n) {
  742.             final int p = n / 2;
  743.             final int q = n - p;
  744.             aOrN[n] = aOrN[p] * aOrN[q];
  745.         }

  747.         return aOrN;

  749.     }
  750.     /** Compute a/r powers array.
  751.      * @param aOr a/r
  752.      * @param <T> type of field used
  753.      * @return array containing (a/r)<sup>n</sup>
  754.      */
  755.     private <T extends CalculusFieldElement<T>> T[] createDistancePowersArray(final T aOr) {

  757.         // initialize array
  758.         final T[] aOrN = MathArrays.buildArray(aOr.getField(), provider.getMaxDegree() + 1);
  759.         aOrN[0] = aOr.getField().getOne();
  760.         if (provider.getMaxDegree() > 0) {
  761.             aOrN[1] = aOr;
  762.         }

  764.         // fill up array
  765.         for (int n = 2; n < aOrN.length; ++n) {
  766.             final int p = n / 2;
  767.             final int q = n - p;
  768.             aOrN[n] = aOrN[p].multiply(aOrN[q]);
  769.         }

  771.         return aOrN;

  773.     }

  775.     /** Compute longitude cosines and sines.
  776.      * @param cosLambda cos(λ)
  777.      * @param sinLambda sin(λ)
  778.      * @return array containing cos(m &times; λ) in row 0
  779.      * and sin(m &times; λ) in row 1
  780.      */
  781.     private double[][] createCosSinArrays(final double cosLambda, final double sinLambda) {

  783.         // initialize arrays
  784.         final double[][] cosSin = new double[2][provider.getMaxOrder() + 1];
  785.         cosSin[0][0] = 1;
  786.         cosSin[1][0] = 0;
  787.         if (provider.getMaxOrder() > 0) {
  788.             cosSin[0][1] = cosLambda;
  789.             cosSin[1][1] = sinLambda;

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

  794.                 // m * lambda is split as p * lambda + q * lambda, trying to avoid
  795.                 // p or q being much larger than the other. This reduces the number of
  796.                 // intermediate results reused to compute each value, and hence should limit
  797.                 // as much as possible roundoff error accumulation
  798.                 // (this does not change the number of floating point operations)
  799.                 final int p = m / 2;
  800.                 final int q = m - p;

  802.                 cosSin[0][m] = cosSin[0][p] * cosSin[0][q] - cosSin[1][p] * cosSin[1][q];
  803.                 cosSin[1][m] = cosSin[1][p] * cosSin[0][q] + cosSin[0][p] * cosSin[1][q];
  804.             }
  805.         }

  807.         return cosSin;

  809.     }

  811.     /** Compute longitude cosines and sines.
  812.      * @param cosLambda cos(λ)
  813.      * @param sinLambda sin(λ)
  814.      * @param <T> type of field used
  815.      * @return array containing cos(m &times; λ) in row 0
  816.      * and sin(m &times; λ) in row 1
  817.      */
  818.     private <T extends CalculusFieldElement<T>> T[][] createCosSinArrays(final T cosLambda, final T sinLambda) {

  820.         final T one = cosLambda.getField().getOne();
  821.         final T zero = cosLambda.getField().getZero();
  822.         // initialize arrays
  823.         final T[][] cosSin = MathArrays.buildArray(one.getField(), 2, provider.getMaxOrder() + 1);
  824.         cosSin[0][0] = one;
  825.         cosSin[1][0] = zero;
  826.         if (provider.getMaxOrder() > 0) {
  827.             cosSin[0][1] = cosLambda;
  828.             cosSin[1][1] = sinLambda;

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

  833.                 // m * lambda is split as p * lambda + q * lambda, trying to avoid
  834.                 // p or q being much larger than the other. This reduces the number of
  835.                 // intermediate results reused to compute each value, and hence should limit
  836.                 // as much as possible roundoff error accumulation
  837.                 // (this does not change the number of floating point operations)
  838.                 final int p = m / 2;
  839.                 final int q = m - p;

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

  844.             }
  845.         }

  847.         return cosSin;

  849.     }

  851.     /** Compute one order of tesseral terms.
  852.      * <p>
  853.      * This corresponds to equations 27 and 30 of the paper.
  854.      * </p>
  855.      * @param m current order
  856.      * @param degree max degree
  857.      * @param index index in the flattened array
  858.      * @param t cos(θ), where θ is the polar angle
  859.      * @param u sin(θ), where θ is the polar angle
  860.      * @param tOu t/u
  861.      * @param pnm0Plus2 array containing scaled P<sub>n,m+2</sub>/u<sup>m+2</sup>
  862.      * @param pnm0Plus1 array containing scaled P<sub>n,m+1</sub>/u<sup>m+1</sup>
  863.      * @param pnm1Plus1 array containing scaled dP<sub>n,m+1</sub>/u<sup>m+1</sup>
  864.      * (may be null if second derivatives are not needed)
  865.      * @param pnm0 array to fill with scaled P<sub>n,m</sub>/u<sup>m</sup>
  866.      * @param pnm1 array to fill with scaled dP<sub>n,m</sub>/u<sup>m</sup>
  867.      * (may be null if first derivatives are not needed)
  868.      * @param pnm2 array to fill with scaled d²P<sub>n,m</sub>/u<sup>m</sup>
  869.      * (may be null if second derivatives are not needed)
  870.      * @return new value for index
  871.      */
  872.     private int computeTesseral(final int m, final int degree, final int index,
  873.                                 final double t, final double u, final double tOu,
  874.                                 final double[] pnm0Plus2, final double[] pnm0Plus1, final double[] pnm1Plus1,
  875.                                 final double[] pnm0, final double[] pnm1, final double[] pnm2) {

  877.         final double u2 = u * u;

  879.         // initialize recursion from sectorial terms
  880.         int n = FastMath.max(2, m);
  881.         if (n == m) {
  882.             pnm0[n] = sectorial[n];
  883.             ++n;
  884.         }

  886.         // compute tesseral values
  887.         int localIndex = index;
  888.         while (n <= degree) {

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

  893.             ++localIndex;
  894.             ++n;

  896.         }

  898.         if (pnm1 != null) {

  900.             // initialize recursion from sectorial terms
  901.             n = FastMath.max(2, m);
  902.             if (n == m) {
  903.                 pnm1[n] = m * tOu * pnm0[n];
  904.                 ++n;
  905.             }

  907.             // compute tesseral values and derivatives with respect to polar angle
  908.             localIndex = index;
  909.             while (n <= degree) {

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

  914.                 ++localIndex;
  915.                 ++n;

  917.             }

  919.             if (pnm2 != null) {

  921.                 // initialize recursion from sectorial terms
  922.                 n = FastMath.max(2, m);
  923.                 if (n == m) {
  924.                     pnm2[n] = m * (tOu * pnm1[n] - pnm0[n] / u2);
  925.                     ++n;
  926.                 }

  928.                 // compute tesseral values and derivatives with respect to polar angle
  929.                 localIndex = index;
  930.                 while (n <= degree) {

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

  935.                     ++localIndex;
  936.                     ++n;

  938.                 }

  940.             }

  942.         }

  944.         return localIndex;

  946.     }

  948.     /** Compute one order of tesseral terms.
  949.      * <p>
  950.      * This corresponds to equations 27 and 30 of the paper.
  951.      * </p>
  952.      * @param m current order
  953.      * @param degree max degree
  954.      * @param index index in the flattened array
  955.      * @param t cos(θ), where θ is the polar angle
  956.      * @param u sin(θ), where θ is the polar angle
  957.      * @param tOu t/u
  958.      * @param pnm0Plus2 array containing scaled P<sub>n,m+2</sub>/u<sup>m+2</sup>
  959.      * @param pnm0Plus1 array containing scaled P<sub>n,m+1</sub>/u<sup>m+1</sup>
  960.      * @param pnm1Plus1 array containing scaled dP<sub>n,m+1</sub>/u<sup>m+1</sup>
  961.      * (may be null if second derivatives are not needed)
  962.      * @param pnm0 array to fill with scaled P<sub>n,m</sub>/u<sup>m</sup>
  963.      * @param pnm1 array to fill with scaled dP<sub>n,m</sub>/u<sup>m</sup>
  964.      * (may be null if first derivatives are not needed)
  965.      * @param pnm2 array to fill with scaled d²P<sub>n,m</sub>/u<sup>m</sup>
  966.      * (may be null if second derivatives are not needed)
  967.      * @param <T> instance of field element
  968.      * @return new value for index
  969.      */
  970.     private <T extends CalculusFieldElement<T>> int computeTesseral(final int m, final int degree, final int index,
  971.                                                                 final T t, final T u, final T tOu,
  972.                                                                 final T[] pnm0Plus2, final T[] pnm0Plus1, final T[] pnm1Plus1,
  973.                                                                 final T[] pnm0, final T[] pnm1, final T[] pnm2) {

  975.         final T u2 = u.square();
  976.         final T zero = u.getField().getZero();
  977.         // initialize recursion from sectorial terms
  978.         int n = FastMath.max(2, m);
  979.         if (n == m) {
  980.             pnm0[n] = zero.newInstance(sectorial[n]);
  981.             ++n;
  982.         }

  984.         // compute tesseral values
  985.         int localIndex = index;
  986.         while (n <= degree) {

  988.             // value (equation 27 of the paper)
  989.             pnm0[n] = t.multiply(gnmOj[localIndex]).multiply(pnm0Plus1[n]).subtract(u2.multiply(pnm0Plus2[n]).multiply(hnmOj[localIndex]));
  990.             ++localIndex;
  991.             ++n;

  993.         }
  994.         if (pnm1 != null) {

  996.             // initialize recursion from sectorial terms
  997.             n = FastMath.max(2, m);
  998.             if (n == m) {
  999.                 pnm1[n] = tOu.multiply(m).multiply(pnm0[n]);
  1000.                 ++n;
  1001.             }

  1003.             // compute tesseral values and derivatives with respect to polar angle
  1004.             localIndex = index;
  1005.             while (n <= degree) {

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

  1010.                 ++localIndex;
  1011.                 ++n;

  1013.             }

  1015.             if (pnm2 != null) {

  1017.                 // initialize recursion from sectorial terms
  1018.                 n = FastMath.max(2, m);
  1019.                 if (n == m) {
  1020.                     pnm2[n] =   tOu.multiply(pnm1[n]).subtract(pnm0[n].divide(u2)).multiply(m);
  1021.                     ++n;
  1022.                 }

  1024.                 // compute tesseral values and derivatives with respect to polar angle
  1025.                 localIndex = index;
  1026.                 while (n <= degree) {

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

  1033.                 }

  1035.             }

  1037.         }
  1038.         return localIndex;

  1040.     }

  1042.     /** {@inheritDoc} */
  1043.     @Override
  1044.     public Vector3D acceleration(final SpacecraftState s, final double[] parameters) {

  1046.         final double mu = parameters[0];

  1048.         // get the position in body frame
  1049.         final AbsoluteDate date       = s.getDate();
  1050.         final StaticTransform fromBodyFrame = bodyFrame.getStaticTransformTo(s.getFrame(), date);
  1051.         final StaticTransform toBodyFrame   = fromBodyFrame.getInverse();
  1052.         final Vector3D position       = toBodyFrame.transformPosition(s.getPosition());

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

  1057.     }

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

  1063.         final T mu = parameters[0];

  1065.         // check for faster computation dedicated to derivatives with respect to state
  1066.         if (isGradientStateDerivative(s)) {
  1067.             @SuppressWarnings("unchecked")
  1068.             final FieldVector3D<Gradient> p = (FieldVector3D<Gradient>) s.getPosition();
  1069.             @SuppressWarnings("unchecked")
  1070.             final FieldVector3D<T> a = (FieldVector3D<T>) accelerationWrtState(s.getDate().toAbsoluteDate(),
  1071.                                                                                s.getFrame(), p,
  1072.                                                                                (Gradient) mu);
  1073.             return a;
  1074.         } else if (isDSStateDerivative(s)) {
  1075.             @SuppressWarnings("unchecked")
  1076.             final FieldVector3D<DerivativeStructure> p = (FieldVector3D<DerivativeStructure>) s.getPosition();
  1077.             @SuppressWarnings("unchecked")
  1078.             final FieldVector3D<T> a = (FieldVector3D<T>) accelerationWrtState(s.getDate().toAbsoluteDate(),
  1079.                                                                                s.getFrame(), p,
  1080.                                                                                (DerivativeStructure) mu);
  1081.             return a;
  1082.         }

  1084.         // get the position in body frame
  1085.         final FieldAbsoluteDate<T> date             = s.getDate();
  1086.         final FieldStaticTransform<T> fromBodyFrame = bodyFrame.getStaticTransformTo(s.getFrame(), date);
  1087.         final FieldStaticTransform<T> toBodyFrame   = fromBodyFrame.getInverse();
  1088.         final FieldVector3D<T> position             = toBodyFrame.transformPosition(s.getPosition());

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

  1093.     }

  1095.     /** Check if a field state corresponds to derivatives with respect to state.
  1096.      * @param state state to check
  1097.      * @param <T> type of the filed elements
  1098.      * @return true if state corresponds to derivatives with respect to state
  1099.      * @since 9.0
  1100.      */
  1101.     private <T extends CalculusFieldElement<T>> boolean isDSStateDerivative(final FieldSpacecraftState<T> state) {
  1102.         try {
  1103.             final DerivativeStructure dsMass = (DerivativeStructure) state.getMass();
  1104.             final int o = dsMass.getOrder();
  1105.             final int p = dsMass.getFreeParameters();
  1106.             if (o != 1 || p < 3) {
  1107.                 return false;
  1108.             }
  1109.             @SuppressWarnings("unchecked")
  1110.             final FieldPVCoordinates<DerivativeStructure> pv = (FieldPVCoordinates<DerivativeStructure>) state.getPVCoordinates();
  1111.             return isVariable(pv.getPosition().getX(), 0) &&
  1112.                    isVariable(pv.getPosition().getY(), 1) &&
  1113.                    isVariable(pv.getPosition().getZ(), 2);
  1114.         } catch (ClassCastException cce) {
  1115.             return false;
  1116.         }
  1117.     }

  1119.     /** Check if a field state corresponds to derivatives with respect to state.
  1120.      * @param state state to check
  1121.      * @param <T> type of the filed elements
  1122.      * @return true if state corresponds to derivatives with respect to state
  1123.      * @since 10.2
  1124.      */
  1125.     private <T extends CalculusFieldElement<T>> boolean isGradientStateDerivative(final FieldSpacecraftState<T> state) {
  1126.         try {
  1127.             final Gradient gMass = (Gradient) state.getMass();
  1128.             final int p = gMass.getFreeParameters();
  1129.             if (p < 3) {
  1130.                 return false;
  1131.             }
  1132.             @SuppressWarnings("unchecked")
  1133.             final FieldPVCoordinates<Gradient> pv = (FieldPVCoordinates<Gradient>) state.getPVCoordinates();
  1134.             return isVariable(pv.getPosition().getX(), 0) &&
  1135.                    isVariable(pv.getPosition().getY(), 1) &&
  1136.                    isVariable(pv.getPosition().getZ(), 2);
  1137.         } catch (ClassCastException cce) {
  1138.             return false;
  1139.         }
  1140.     }

  1142.     /** Check if a derivative represents a specified variable.
  1143.      * @param ds derivative to check
  1144.      * @param index index of the variable
  1145.      * @return true if the derivative represents a specified variable
  1146.      * @since 9.0
  1147.      */
  1148.     private boolean isVariable(final DerivativeStructure ds, final int index) {
  1149.         final double[] derivatives = ds.getAllDerivatives();
  1150.         boolean check = true;
  1151.         for (int i = 1; i < derivatives.length; ++i) {
  1152.             check &= derivatives[i] == ((index + 1 == i) ? 1.0 : 0.0);
  1153.         }
  1154.         return check;
  1155.     }

  1157.     /** Check if a derivative represents a specified variable.
  1158.      * @param g derivative to check
  1159.      * @param index index of the variable
  1160.      * @return true if the derivative represents a specified variable
  1161.      * @since 10.2
  1162.      */
  1163.     private boolean isVariable(final Gradient g, final int index) {
  1164.         final double[] derivatives = g.getGradient();
  1165.         boolean check = true;
  1166.         for (int i = 0; i < derivatives.length; ++i) {
  1167.             check &= derivatives[i] == ((index == i) ? 1.0 : 0.0);
  1168.         }
  1169.         return check;
  1170.     }

  1172.     /** Compute acceleration derivatives with respect to state parameters.
  1173.      * <p>
  1174.      * From a theoretical point of view, this method computes the same values
  1175.      * as {@link #acceleration(FieldSpacecraftState, CalculusFieldElement[])} in the
  1176.      * specific case of {@link DerivativeStructure} with respect to state, so
  1177.      * it is less general. However, it is *much* faster in this important case.
  1178.      * <p>
  1179.      * <p>
  1180.      * The derivatives should be computed with respect to position. The input
  1181.      * parameters already take into account the free parameters (6 or 7 depending
  1182.      * on derivation with respect to mass being considered or not) and order
  1183.      * (always 1). Free parameters at indices 0, 1 and 2 correspond to derivatives
  1184.      * with respect to position. Free parameters at indices 3, 4 and 5 correspond
  1185.      * to derivatives with respect to velocity (these derivatives will remain zero
  1186.      * as acceleration due to gravity does not depend on velocity). Free parameter
  1187.      * at index 6 (if present) corresponds to to derivatives with respect to mass
  1188.      * (this derivative will remain zero as acceleration due to gravity does not
  1189.      * depend on mass).
  1190.      * </p>
  1191.      * @param date current date
  1192.      * @param frame inertial reference frame for state (both orbit and attitude)
  1193.      * @param position position of spacecraft in inertial frame
  1194.      * @param mu central attraction coefficient to use
  1195.      * @return acceleration with all derivatives specified by the input parameters
  1196.      * own derivatives
  1197.      * @since 6.0
  1198.      */
  1199.     private FieldVector3D<DerivativeStructure> accelerationWrtState(final AbsoluteDate date, final Frame frame,
  1200.                                                                     final FieldVector3D<DerivativeStructure> position,
  1201.                                                                     final DerivativeStructure mu) {

  1203.         // free parameters
  1204.         final int freeParameters = mu.getFreeParameters();

  1206.         // get the position in body frame
  1207.         final StaticTransform fromBodyFrame = bodyFrame.getStaticTransformTo(frame, date);
  1208.         final StaticTransform toBodyFrame   = fromBodyFrame.getInverse();
  1209.         final Vector3D positionBody   = toBodyFrame.transformPosition(position.toVector3D());

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

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

  1217.         // Hessian of the non-central part of the gravity field
  1218.         final RealMatrix hBody     = new Array2DRowRealMatrix(gh.getHessian(), false);
  1219.         final RealMatrix rot       = new Array2DRowRealMatrix(toBodyFrame.getRotation().getMatrix());
  1220.         final RealMatrix hInertial = rot.transposeMultiply(hBody).multiply(rot);

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

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

  1230.             // Jacobian of acceleration (i.e. Hessian of field)
  1231.             derivatives[1] = hInertial.getEntry(i, 0);
  1232.             derivatives[2] = hInertial.getEntry(i, 1);
  1233.             derivatives[3] = hInertial.getEntry(i, 2);

  1235.             // next element is derivative with respect to parameter mu
  1236.             if (derivatives.length > 4 && isVariable(mu, 3)) {
  1237.                 derivatives[4] = gInertial[i] / mu.getReal();
  1238.             }

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

  1242.         }

  1244.         return new FieldVector3D<>(accDer);

  1246.     }

  1248.     /** Compute acceleration derivatives with respect to state parameters.
  1249.      * <p>
  1250.      * From a theoretical point of view, this method computes the same values
  1251.      * as {@link #acceleration(FieldSpacecraftState, CalculusFieldElement[])} in the
  1252.      * specific case of {@link DerivativeStructure} with respect to state, so
  1253.      * it is less general. However, it is *much* faster in this important case.
  1254.      * <p>
  1255.      * <p>
  1256.      * The derivatives should be computed with respect to position. The input
  1257.      * parameters already take into account the free parameters (6 or 7 depending
  1258.      * on derivation with respect to mass being considered or not) and order
  1259.      * (always 1). Free parameters at indices 0, 1 and 2 correspond to derivatives
  1260.      * with respect to position. Free parameters at indices 3, 4 and 5 correspond
  1261.      * to derivatives with respect to velocity (these derivatives will remain zero
  1262.      * as acceleration due to gravity does not depend on velocity). Free parameter
  1263.      * at index 6 (if present) corresponds to to derivatives with respect to mass
  1264.      * (this derivative will remain zero as acceleration due to gravity does not
  1265.      * depend on mass).
  1266.      * </p>
  1267.      * @param date current date
  1268.      * @param frame inertial reference frame for state (both orbit and attitude)
  1269.      * @param position position of spacecraft in inertial frame
  1270.      * @param mu central attraction coefficient to use
  1271.      * @return acceleration with all derivatives specified by the input parameters
  1272.      * own derivatives
  1273.      * @since 10.2
  1274.      */
  1275.     private FieldVector3D<Gradient> accelerationWrtState(final AbsoluteDate date, final Frame frame,
  1276.                                                          final FieldVector3D<Gradient> position,
  1277.                                                          final Gradient mu) {

  1279.         // free parameters
  1280.         final int freeParameters = mu.getFreeParameters();

  1282.         // get the position in body frame
  1283.         final StaticTransform fromBodyFrame = bodyFrame.getStaticTransformTo(frame, date);
  1284.         final StaticTransform toBodyFrame   = fromBodyFrame.getInverse();
  1285.         final Vector3D positionBody   = toBodyFrame.transformPosition(position.toVector3D());

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

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

  1293.         // Hessian of the non-central part of the gravity field
  1294.         final RealMatrix hBody     = new Array2DRowRealMatrix(gh.getHessian(), false);
  1295.         final RealMatrix rot       = new Array2DRowRealMatrix(toBodyFrame.getRotation().getMatrix());
  1296.         final RealMatrix hInertial = rot.transposeMultiply(hBody).multiply(rot);

  1298.         // distribute all partial derivatives in a compact acceleration vector
  1299.         final double[] derivatives = new double[freeParameters];
  1300.         final Gradient[] accDer = new Gradient[3];
  1301.         for (int i = 0; i < 3; ++i) {

  1303.             // Jacobian of acceleration (i.e. Hessian of field)
  1304.             derivatives[0] = hInertial.getEntry(i, 0);
  1305.             derivatives[1] = hInertial.getEntry(i, 1);
  1306.             derivatives[2] = hInertial.getEntry(i, 2);

  1308.             // next element is derivative with respect to parameter mu
  1309.             if (derivatives.length > 3 && isVariable(mu, 3)) {
  1310.                 derivatives[3] = gInertial[i] / mu.getReal();
  1311.             }

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

  1315.         }

  1317.         return new FieldVector3D<>(accDer);

  1319.     }

  1321.     /** {@inheritDoc} */
  1322.     public List<ParameterDriver> getParametersDrivers() {
  1323.         return Collections.singletonList(gmParameterDriver);
  1324.     }

  1326. }
Created with JaCoCo 0.8.12.202403310830