HolmesFeatherstoneAttractionModel.java

  1. /* Copyright 2002-2013 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.forces.gravity;


  20. import java.io.Serializable;

  22. import org.apache.commons.math3.analysis.differentiation.DerivativeStructure;
  23. import org.apache.commons.math3.geometry.euclidean.threed.FieldRotation;
  24. import org.apache.commons.math3.geometry.euclidean.threed.FieldVector3D;
  25. import org.apache.commons.math3.geometry.euclidean.threed.SphericalCoordinates;
  26. import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
  27. import org.apache.commons.math3.linear.Array2DRowRealMatrix;
  28. import org.apache.commons.math3.linear.RealMatrix;
  29. import org.apache.commons.math3.ode.AbstractParameterizable;
  30. import org.apache.commons.math3.util.FastMath;
  31. import org.orekit.errors.OrekitException;
  32. import org.orekit.forces.ForceModel;
  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.Transform;
  39. import org.orekit.propagation.SpacecraftState;
  40. import org.orekit.propagation.events.EventDetector;
  41. import org.orekit.propagation.numerical.TimeDerivativesEquations;
  42. import org.orekit.time.AbsoluteDate;

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

  72. public class HolmesFeatherstoneAttractionModel
  73.     extends AbstractParameterizable implements ForceModel, TideSystemProvider {

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

  81.     /** Provider for the spherical harmonics. */
  82.     private final NormalizedSphericalHarmonicsProvider provider;

  84.     /** Central attraction coefficient. */
  85.     private double mu;

  87.     /** Rotating body. */
  88.     private final Frame bodyFrame;

  90.     /** Recursion coefficients g<sub>n,m</sub>/&radic;j. */
  91.     private final double[] gnmOj;

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

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

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

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

  110.         super(NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT);

  112.         this.provider  = provider;
  113.         this.mu        = provider.getMu();
  114.         this.bodyFrame = centralBodyFrame;

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

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

  140.         // scaled sectorial terms corresponding to equation 28 in Holmes and Featherstone paper
  141.         sectorial    = new double[degree + 1];
  142.         sectorial[0] = FastMath.scalb(1.0, -SCALING);
  143.         sectorial[1] = FastMath.sqrt(3) * sectorial[0];
  144.         for (int m = 2; m < sectorial.length; ++m) {
  145.             sectorial[m] = FastMath.sqrt((2 * m + 1) / (2.0 * m)) * sectorial[m - 1];
  146.         }

  148.     }

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

  155.     /** Compute the value of the gravity field.
  156.      * @param date current date
  157.      * @param position position at which gravity field is desired in body frame
  158.      * @return value of the gravity field (central and non-central parts summed together)
  159.      * @exception OrekitException if position cannot be converted to central body frame
  160.      */
  161.     public double value(final AbsoluteDate date, final Vector3D position)
  162.         throws OrekitException {
  163.         return mu / position.getNorm() + nonCentralPart(date, position);
  164.     }

  166.     /** Compute the non-central part of the gravity field.
  167.      * @param date current date
  168.      * @param position position at which gravity field is desired in body frame
  169.      * @return value of the non-central part of the gravity field
  170.      * @exception OrekitException if position cannot be converted to central body frame
  171.      */
  172.     public double nonCentralPart(final AbsoluteDate date, final Vector3D position)
  173.         throws OrekitException {

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

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

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

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

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

  204.         // outer summation over order
  205.         int    index = 0;
  206.         double value = 0;
  207.         for (int m = degree; m >= 0; --m) {

  209.             // compute tesseral terms without derivatives
  210.             index = computeTesseral(m, degree, index, t, u, tOu,
  211.                                     pnm0Plus2, pnm0Plus1, null, pnm0, null, null);

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

  216.                 // inner summation over degree, for fixed order
  217.                 double sumDegreeS        = 0;
  218.                 double sumDegreeC        = 0;
  219.                 for (int n = FastMath.max(2, m); n <= degree; ++n) {
  220.                     sumDegreeS += pnm0[n] * aOrN[n] * harmonics.getNormalizedSnm(n, m);
  221.                     sumDegreeC += pnm0[n] * aOrN[n] * harmonics.getNormalizedCnm(n, m);
  222.                 }

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

  227.             }

  229.             // rotate the recursion arrays
  230.             final double[] tmp = pnm0Plus2;
  231.             pnm0Plus2 = pnm0Plus1;
  232.             pnm0Plus1 = pnm0;
  233.             pnm0      = tmp;

  235.         }

  237.         // scale back
  238.         value = FastMath.scalb(value, SCALING);

  240.         // apply the global mu/r factor
  241.         return mu * value / r;

  243.     }

  245.     /** Compute the gradient of the non-central part of the gravity field.
  246.      * @param date current date
  247.      * @param position position at which gravity field is desired in body frame
  248.      * @return gradient of the non-central part of the gravity field
  249.      * @exception OrekitException if position cannot be converted to central body frame
  250.      */
  251.     public double[] gradient(final AbsoluteDate date, final Vector3D position)
  252.         throws OrekitException {

  254.         final int degree = provider.getMaxDegree();
  255.         final int order  = provider.getMaxOrder();
  256.         final NormalizedSphericalHarmonics harmonics = provider.onDate(date);

  258.         // allocate the columns for recursion
  259.         double[] pnm0Plus2  = new double[degree + 1];
  260.         double[] pnm0Plus1  = new double[degree + 1];
  261.         double[] pnm0       = new double[degree + 1];
  262.         final double[] pnm1 = new double[degree + 1];

  264.         // compute polar coordinates
  265.         final double x    = position.getX();
  266.         final double y    = position.getY();
  267.         final double z    = position.getZ();
  268.         final double x2   = x * x;
  269.         final double y2   = y * y;
  270.         final double z2   = z * z;
  271.         final double r2   = x2 + y2 + z2;
  272.         final double r    = FastMath.sqrt (r2);
  273.         final double rho2 = x2 + y2;
  274.         final double rho  = FastMath.sqrt(rho2);
  275.         final double t    = z / r;   // cos(theta), where theta is the polar angle
  276.         final double u    = rho / r; // sin(theta), where theta is the polar angle
  277.         final double tOu  = z / rho;

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

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

  285.         // outer summation over order
  286.         int    index = 0;
  287.         double value = 0;
  288.         final double[] gradient = new double[3];
  289.         for (int m = degree; m >= 0; --m) {

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

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

  298.                 // inner summation over degree, for fixed order
  299.                 double sumDegreeS        = 0;
  300.                 double sumDegreeC        = 0;
  301.                 double dSumDegreeSdR     = 0;
  302.                 double dSumDegreeCdR     = 0;
  303.                 double dSumDegreeSdTheta = 0;
  304.                 double dSumDegreeCdTheta = 0;
  305.                 for (int n = FastMath.max(2, m); n <= degree; ++n) {
  306.                     final double qSnm  = aOrN[n] * harmonics.getNormalizedSnm(n, m);
  307.                     final double qCnm  = aOrN[n] * harmonics.getNormalizedCnm(n, m);
  308.                     final double nOr   = n / r;
  309.                     final double s0    = pnm0[n] * qSnm;
  310.                     final double c0    = pnm0[n] * qCnm;
  311.                     final double s1    = pnm1[n] * qSnm;
  312.                     final double c1    = pnm1[n] * qCnm;
  313.                     sumDegreeS        += s0;
  314.                     sumDegreeC        += c0;
  315.                     dSumDegreeSdR     -= nOr * s0;
  316.                     dSumDegreeCdR     -= nOr * c0;
  317.                     dSumDegreeSdTheta += s1;
  318.                     dSumDegreeCdTheta += c1;
  319.                 }

  321.                 // contribution to outer summation over order
  322.                 // beware that we need to order gradient using the mathematical conventions
  323.                 // compliant with the SphericalCoordinates class, so our lambda is its theta
  324.                 // (and hence at index 1) and our theta is its phi (and hence at index 2)
  325.                 final double sML = cosSinLambda[1][m];
  326.                 final double cML = cosSinLambda[0][m];
  327.                 value            = value       * u + sML * sumDegreeS        + cML * sumDegreeC;
  328.                 gradient[0]      = gradient[0] * u + sML * dSumDegreeSdR     + cML * dSumDegreeCdR;
  329.                 gradient[1]      = gradient[1] * u + m * (cML * sumDegreeS - sML * sumDegreeC);
  330.                 gradient[2]      = gradient[2] * u + sML * dSumDegreeSdTheta + cML * dSumDegreeCdTheta;

  332.             }

  334.             // rotate the recursion arrays
  335.             final double[] tmp = pnm0Plus2;
  336.             pnm0Plus2 = pnm0Plus1;
  337.             pnm0Plus1 = pnm0;
  338.             pnm0      = tmp;

  340.         }

  342.         // scale back
  343.         value       = FastMath.scalb(value,       SCALING);
  344.         gradient[0] = FastMath.scalb(gradient[0], SCALING);
  345.         gradient[1] = FastMath.scalb(gradient[1], SCALING);
  346.         gradient[2] = FastMath.scalb(gradient[2], SCALING);

  348.         // apply the global mu/r factor
  349.         final double muOr = mu / r;
  350.         value            *= muOr;
  351.         gradient[0]       = muOr * gradient[0] - value / r;
  352.         gradient[1]      *= muOr;
  353.         gradient[2]      *= muOr;

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

  358.     }

  360.     /** Compute both the gradient and the hessian of the non-central part of the gravity field.
  361.      * @param date current date
  362.      * @param position position at which gravity field is desired in body frame
  363.      * @return gradient and hessian of the non-central part of the gravity field
  364.      * @exception OrekitException if position cannot be converted to central body frame
  365.      */
  366.     public GradientHessian gradientHessian(final AbsoluteDate date, final Vector3D position)
  367.         throws OrekitException {

  369.         final int degree = provider.getMaxDegree();
  370.         final int order  = provider.getMaxOrder();
  371.         final NormalizedSphericalHarmonics harmonics = provider.onDate(date);

  373.         // allocate the columns for recursion
  374.         double[] pnm0Plus2  = new double[degree + 1];
  375.         double[] pnm0Plus1  = new double[degree + 1];
  376.         double[] pnm0       = new double[degree + 1];
  377.         double[] pnm1Plus1  = new double[degree + 1];
  378.         double[] pnm1       = new double[degree + 1];
  379.         final double[] pnm2 = new double[degree + 1];

  381.         // compute polar coordinates
  382.         final double x    = position.getX();
  383.         final double y    = position.getY();
  384.         final double z    = position.getZ();
  385.         final double x2   = x * x;
  386.         final double y2   = y * y;
  387.         final double z2   = z * z;
  388.         final double r2   = x2 + y2 + z2;
  389.         final double r    = FastMath.sqrt (r2);
  390.         final double rho2 = x2 + y2;
  391.         final double rho  = FastMath.sqrt(rho2);
  392.         final double t    = z / r;   // cos(theta), where theta is the polar angle
  393.         final double u    = rho / r; // sin(theta), where theta is the polar angle
  394.         final double tOu  = z / rho;

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

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

  402.         // outer summation over order
  403.         int    index = 0;
  404.         double value = 0;
  405.         final double[]   gradient = new double[3];
  406.         final double[][] hessian  = new double[3][3];
  407.         for (int m = degree; m >= 0; --m) {

  409.             // compute tesseral terms
  410.             index = computeTesseral(m, degree, index, t, u, tOu,
  411.                                     pnm0Plus2, pnm0Plus1, pnm1Plus1, pnm0, pnm1, pnm2);

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

  416.                 // inner summation over degree, for fixed order
  417.                 double sumDegreeS               = 0;
  418.                 double sumDegreeC               = 0;
  419.                 double dSumDegreeSdR            = 0;
  420.                 double dSumDegreeCdR            = 0;
  421.                 double dSumDegreeSdTheta        = 0;
  422.                 double dSumDegreeCdTheta        = 0;
  423.                 double d2SumDegreeSdRdR         = 0;
  424.                 double d2SumDegreeSdRdTheta     = 0;
  425.                 double d2SumDegreeSdThetadTheta = 0;
  426.                 double d2SumDegreeCdRdR         = 0;
  427.                 double d2SumDegreeCdRdTheta     = 0;
  428.                 double d2SumDegreeCdThetadTheta = 0;
  429.                 for (int n = FastMath.max(2, m); n <= degree; ++n) {
  430.                     final double qSnm         = aOrN[n] * harmonics.getNormalizedSnm(n, m);
  431.                     final double qCnm         = aOrN[n] * harmonics.getNormalizedCnm(n, m);
  432.                     final double nOr          = n / r;
  433.                     final double nnP1Or2      = nOr * (n + 1) / r;
  434.                     final double s0           = pnm0[n] * qSnm;
  435.                     final double c0           = pnm0[n] * qCnm;
  436.                     final double s1           = pnm1[n] * qSnm;
  437.                     final double c1           = pnm1[n] * qCnm;
  438.                     final double s2           = pnm2[n] * qSnm;
  439.                     final double c2           = pnm2[n] * qCnm;
  440.                     sumDegreeS               += s0;
  441.                     sumDegreeC               += c0;
  442.                     dSumDegreeSdR            -= nOr * s0;
  443.                     dSumDegreeCdR            -= nOr * c0;
  444.                     dSumDegreeSdTheta        += s1;
  445.                     dSumDegreeCdTheta        += c1;
  446.                     d2SumDegreeSdRdR         += nnP1Or2 * s0;
  447.                     d2SumDegreeSdRdTheta     -= nOr * s1;
  448.                     d2SumDegreeSdThetadTheta += s2;
  449.                     d2SumDegreeCdRdR         += nnP1Or2 * c0;
  450.                     d2SumDegreeCdRdTheta     -= nOr * c1;
  451.                     d2SumDegreeCdThetadTheta += c2;
  452.                 }

  454.                 // contribution to outer summation over order
  455.                 final double sML = cosSinLambda[1][m];
  456.                 final double cML = cosSinLambda[0][m];
  457.                 value            = value         * u + sML * sumDegreeS + cML * sumDegreeC;
  458.                 gradient[0]      = gradient[0]   * u + sML * dSumDegreeSdR + cML * dSumDegreeCdR;
  459.                 gradient[1]      = gradient[1]   * u + m * (cML * sumDegreeS - sML * sumDegreeC);
  460.                 gradient[2]      = gradient[2]   * u + sML * dSumDegreeSdTheta + cML * dSumDegreeCdTheta;
  461.                 hessian[0][0]    = hessian[0][0] * u + sML * d2SumDegreeSdRdR + cML * d2SumDegreeCdRdR;
  462.                 hessian[1][0]    = hessian[1][0] * u + m * (cML * dSumDegreeSdR - sML * dSumDegreeCdR);
  463.                 hessian[2][0]    = hessian[2][0] * u + sML * d2SumDegreeSdRdTheta + cML * d2SumDegreeCdRdTheta;
  464.                 hessian[1][1]    = hessian[1][1] * u - m * m * (sML * sumDegreeS + cML * sumDegreeC);
  465.                 hessian[2][1]    = hessian[2][1] * u + m * (cML * dSumDegreeSdTheta - sML * dSumDegreeCdTheta);
  466.                 hessian[2][2]    = hessian[2][2] * u + sML * d2SumDegreeSdThetadTheta + cML * d2SumDegreeCdThetadTheta;

  468.             }

  470.             // rotate the recursion arrays
  471.             final double[] tmp0 = pnm0Plus2;
  472.             pnm0Plus2 = pnm0Plus1;
  473.             pnm0Plus1 = pnm0;
  474.             pnm0      = tmp0;
  475.             final double[] tmp1 = pnm1Plus1;
  476.             pnm1Plus1 = pnm1;
  477.             pnm1      = tmp1;

  479.         }

  481.         // scale back
  482.         value = FastMath.scalb(value, SCALING);
  483.         for (int i = 0; i < 3; ++i) {
  484.             gradient[i] = FastMath.scalb(gradient[i], SCALING);
  485.             for (int j = 0; j <= i; ++j) {
  486.                 hessian[i][j] = FastMath.scalb(hessian[i][j], SCALING);
  487.             }
  488.         }


  491.         // apply the global mu/r factor
  492.         final double muOr = mu / r;
  493.         value         *= muOr;
  494.         gradient[0]    = muOr * gradient[0] - value / r;
  495.         gradient[1]   *= muOr;
  496.         gradient[2]   *= muOr;
  497.         hessian[0][0]  = muOr * hessian[0][0] - 2 * gradient[0] / r;
  498.         hessian[1][0]  = muOr * hessian[1][0] -     gradient[1] / r;
  499.         hessian[2][0]  = muOr * hessian[2][0] -     gradient[2] / r;
  500.         hessian[1][1] *= muOr;
  501.         hessian[2][1] *= muOr;
  502.         hessian[2][2] *= muOr;

  504.         // convert gradient and Hessian from spherical to Cartesian
  505.         final SphericalCoordinates sc = new SphericalCoordinates(position);
  506.         return new GradientHessian(sc.toCartesianGradient(gradient),
  507.                                    sc.toCartesianHessian(hessian, gradient));


  510.     }

  512.     /** Container for gradient and Hessian. */
  513.     public static class GradientHessian implements Serializable {

  515.         /** Serializable UID. */
  516.         private static final long serialVersionUID = 20130219L;

  518.         /** Gradient. */
  519.         private final double[] gradient;

  521.         /** Hessian. */
  522.         private final double[][] hessian;

  524.         /** Simple constructor.
  525.          * <p>
  526.          * A reference to the arrays is stored, they are <strong>not</strong> cloned.
  527.          * </p>
  528.          * @param gradient gradient
  529.          * @param hessian hessian
  530.          */
  531.         public GradientHessian(final double[] gradient, final double[][] hessian) {
  532.             this.gradient = gradient;
  533.             this.hessian  = hessian;
  534.         }

  536.         /** Get a reference to the gradient.
  537.          * @return gradient (a reference to the internal array is returned)
  538.          */
  539.         public double[] getGradient() {
  540.             return gradient;
  541.         }

  543.         /** Get a reference to the Hessian.
  544.          * @return Hessian (a reference to the internal array is returned)
  545.          */
  546.         public double[][] getHessian() {
  547.             return hessian;
  548.         }

  550.     }

  552.     /** Compute a/r powers array.
  553.      * @param aOr a/r
  554.      * @return array containing (a/r)<sup>n</sup>
  555.      */
  556.     private double[] createDistancePowersArray(final double aOr) {

  558.         // initialize array
  559.         final double[] aOrN = new double[provider.getMaxDegree() + 1];
  560.         aOrN[0] = 1;
  561.         aOrN[1] = aOr;

  563.         // fill up array
  564.         for (int n = 2; n < aOrN.length; ++n) {
  565.             final int p = n / 2;
  566.             final int q = n - p;
  567.             aOrN[n] = aOrN[p] * aOrN[q];
  568.         }

  570.         return aOrN;

  572.     }

  574.     /** Compute longitude cosines and sines.
  575.      * @param cosLambda cos(&lambda;)
  576.      * @param sinLambda sin(&lambda;)
  577.      * @return array containing cos(m &times; &lambda;) in row 0
  578.      * and sin(m &times; &lambda;) in row 1
  579.      */
  580.     private double[][] createCosSinArrays(final double cosLambda, final double sinLambda) {

  582.         // initialize arrays
  583.         final double[][] cosSin = new double[2][provider.getMaxOrder() + 1];
  584.         cosSin[0][0] = 1;
  585.         cosSin[1][0] = 0;
  586.         if (provider.getMaxOrder() > 0) {
  587.             cosSin[0][1] = cosLambda;
  588.             cosSin[1][1] = sinLambda;

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

  593.                 // m * lambda is split as p * lambda + q * lambda, trying to avoid
  594.                 // p or q being much larger than the other. This reduces the number of
  595.                 // intermediate results reused to compute each value, and hence should limit
  596.                 // as much as possible roundoff error accumulation
  597.                 // (this does not change the number of floating point operations)
  598.                 final int p = m / 2;
  599.                 final int q = m - p;

  601.                 cosSin[0][m] = cosSin[0][p] * cosSin[0][q] - cosSin[1][p] * cosSin[1][q];
  602.                 cosSin[1][m] = cosSin[1][p] * cosSin[0][q] + cosSin[0][p] * cosSin[1][q];

  604.             }
  605.         }

  607.         return cosSin;

  609.     }

  611.     /** Compute one order of tesseral terms.
  612.      * <p>
  613.      * This corresponds to equations 27 and 30 of the paper.
  614.      * </p>
  615.      * @param m current order
  616.      * @param degree max degree
  617.      * @param index index in the flattened array
  618.      * @param t cos(&theta;), where &theta; is the polar angle
  619.      * @param u sin(&theta;), where &theta; is the polar angle
  620.      * @param tOu t/u
  621.      * @param pnm0Plus2 array containing scaled P<sub>n,m+2</sub>/u<sup>m+2</sup>
  622.      * @param pnm0Plus1 array containing scaled P<sub>n,m+1</sub>/u<sup>m+1</sup>
  623.      * @param pnm1Plus1 array containing scaled dP<sub>n,m+1</sub>/u<sup>m+1</sup>
  624.      * (may be null if second derivatives are not needed)
  625.      * @param pnm0 array to fill with scaled P<sub>n,m</sub>/u<sup>m</sup>
  626.      * @param pnm1 array to fill with scaled dP<sub>n,m</sub>/u<sup>m</sup>
  627.      * (may be null if first derivatives are not needed)
  628.      * @param pnm2 array to fill with scaled d<sup>2</sup>P<sub>n,m</sub>/u<sup>m</sup>
  629.      * (may be null if second derivatives are not needed)
  630.      * @return new value for index
  631.      */
  632.     private int computeTesseral(final int m, final int degree, final int index,
  633.                                 final double t, final double u, final double tOu,
  634.                                 final double[] pnm0Plus2, final double[] pnm0Plus1, final double[] pnm1Plus1,
  635.                                 final double[] pnm0, final double[] pnm1, final double[] pnm2) {

  637.         final double u2 = u * u;

  639.         // initialize recursion from sectorial terms
  640.         int n = FastMath.max(2, m);
  641.         if (n == m) {
  642.             pnm0[n] = sectorial[n];
  643.             ++n;
  644.         }

  646.         // compute tesseral values
  647.         int localIndex = index;
  648.         while (n <= degree) {

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

  653.             ++localIndex;
  654.             ++n;

  656.         }

  658.         if (pnm1 != null) {

  660.             // initialize recursion from sectorial terms
  661.             n = FastMath.max(2, m);
  662.             if (n == m) {
  663.                 pnm1[n] = m * tOu * pnm0[n];
  664.                 ++n;
  665.             }

  667.             // compute tesseral values and derivatives with respect to polar angle
  668.             localIndex = index;
  669.             while (n <= degree) {

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

  674.                 ++localIndex;
  675.                 ++n;

  677.             }

  679.             if (pnm2 != null) {

  681.                 // initialize recursion from sectorial terms
  682.                 n = FastMath.max(2, m);
  683.                 if (n == m) {
  684.                     pnm2[n] = m * (tOu * pnm1[n] - pnm0[n] / u2);
  685.                     ++n;
  686.                 }

  688.                 // compute tesseral values and derivatives with respect to polar angle
  689.                 localIndex = index;
  690.                 while (n <= degree) {

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

  695.                     ++localIndex;
  696.                     ++n;

  698.                 }

  700.             }

  702.         }

  704.         return localIndex;

  706.     }

  708.     /** {@inheritDoc} */
  709.     public void addContribution(final SpacecraftState s, final TimeDerivativesEquations adder)
  710.         throws OrekitException {

  712.         // get the position in body frame
  713.         final AbsoluteDate date       = s.getDate();
  714.         final Transform fromBodyFrame = bodyFrame.getTransformTo(s.getFrame(), date);
  715.         final Transform toBodyFrame   = fromBodyFrame.getInverse();
  716.         final Vector3D position       = toBodyFrame.transformPosition(s.getPVCoordinates().getPosition());

  718.         // gradient of the non-central part of the gravity field
  719.         final Vector3D gInertial = fromBodyFrame.transformVector(new Vector3D(gradient(date, position)));

  721.         adder.addXYZAcceleration(gInertial.getX(), gInertial.getY(), gInertial.getZ());

  723.     }

  725.     /** {@inheritDoc} */
  726.     public EventDetector[] getEventsDetectors() {
  727.         return new EventDetector[0];
  728.     }

  730.     /** {@inheritDoc} */
  731.     public double getParameter(final String name)
  732.         throws IllegalArgumentException {
  733.         complainIfNotSupported(name);
  734.         return mu;
  735.     }

  737.     /** {@inheritDoc} */
  738.     public void setParameter(final String name, final double value)
  739.         throws IllegalArgumentException {
  740.         complainIfNotSupported(name);
  741.         mu = value;
  742.     }

  744.     /** {@inheritDoc} */
  745.     public FieldVector3D<DerivativeStructure> accelerationDerivatives(final AbsoluteDate date, final Frame frame,
  746.                                                                       final FieldVector3D<DerivativeStructure> position, final FieldVector3D<DerivativeStructure> velocity,
  747.                                                                       final FieldRotation<DerivativeStructure> rotation, final DerivativeStructure mass)
  748.         throws OrekitException {

  750.         // get the position in body frame
  751.         final Transform fromBodyFrame = bodyFrame.getTransformTo(frame, date);
  752.         final Transform toBodyFrame   = fromBodyFrame.getInverse();
  753.         final Vector3D positionBody   = toBodyFrame.transformPosition(position.toVector3D());

  755.         // compute gradient and Hessian
  756.         final GradientHessian gh   = gradientHessian(date, positionBody);

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

  761.         // Hessian of the non-central part of the gravity field
  762.         final RealMatrix hBody     = new Array2DRowRealMatrix(gh.getHessian(), false);
  763.         final RealMatrix rot       = new Array2DRowRealMatrix(toBodyFrame.getRotation().getMatrix());
  764.         final RealMatrix hInertial = rot.transpose().multiply(hBody).multiply(rot);

  766.         // distribute all partial derivatives in a compact acceleration vector
  767.         final int parameters       = mass.getFreeParameters();
  768.         final int order            = mass.getOrder();
  769.         final double[] derivatives = new double[1 + parameters];
  770.         final DerivativeStructure[] accDer = new DerivativeStructure[3];
  771.         for (int i = 0; i < 3; ++i) {

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

  776.             // next three elements are one row of the Jacobian of acceleration (i.e. Hessian of field)
  777.             derivatives[1] = hInertial.getEntry(i, 0);
  778.             derivatives[2] = hInertial.getEntry(i, 1);
  779.             derivatives[3] = hInertial.getEntry(i, 2);

  781.             // next elements (three or four depending on mass being used or not) are left as 0

  783.             accDer[i] = new DerivativeStructure(parameters, order, derivatives);

  785.         }

  787.         return new FieldVector3D<DerivativeStructure>(accDer);

  789.     }

  791.     /** {@inheritDoc} */
  792.     public FieldVector3D<DerivativeStructure> accelerationDerivatives(final SpacecraftState s, final String paramName)
  793.         throws OrekitException, IllegalArgumentException {

  795.         complainIfNotSupported(paramName);

  797.         // get the position in body frame
  798.         final AbsoluteDate date       = s.getDate();
  799.         final Transform fromBodyFrame = bodyFrame.getTransformTo(s.getFrame(), date);
  800.         final Transform toBodyFrame   = fromBodyFrame.getInverse();
  801.         final Vector3D position       = toBodyFrame.transformPosition(s.getPVCoordinates().getPosition());

  803.         // gradient of the non-central part of the gravity field
  804.         final Vector3D gInertial = fromBodyFrame.transformVector(new Vector3D(gradient(date, position)));

  806.         return new FieldVector3D<DerivativeStructure>(new DerivativeStructure(1, 1, gInertial.getX(), gInertial.getX() / mu),
  807.                               new DerivativeStructure(1, 1, gInertial.getY(), gInertial.getY() / mu),
  808.                               new DerivativeStructure(1, 1, gInertial.getZ(), gInertial.getZ() / mu));

  810.     }

  812. }
Created with JaCoCo 0.6.3.201306030806