FieldDeepSDP4.java

  1. /* Copyright 2002-2025 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.propagation.analytical.tle;

  19. import org.hipparchus.CalculusFieldElement;
  20. import org.hipparchus.util.FastMath;
  21. import org.hipparchus.util.FieldSinCos;
  22. import org.hipparchus.util.MathArrays;
  23. import org.hipparchus.util.MathUtils;
  24. import org.hipparchus.util.SinCos;
  25. import org.orekit.annotation.DefaultDataContext;
  26. import org.orekit.attitudes.AttitudeProvider;
  27. import org.orekit.data.DataContext;
  28. import org.orekit.frames.Frame;
  29. import org.orekit.time.DateTimeComponents;
  30. import org.orekit.utils.Constants;


  33. /** This class contains the methods that compute deep space perturbation terms.
  34.  * <p>
  35.  * The user should not bother in this class since it is handled internaly by the
  36.  * {@link TLEPropagator}.
  37.  * </p>
  38.  * <p>This implementation is largely inspired from the paper and source code <a
  39.  * href="https://www.celestrak.com/publications/AIAA/2006-6753/">Revisiting Spacetrack
  40.  * Report #3</a> and is fully compliant with its results and tests cases.</p>
  41.  * @author Felix R. Hoots, Ronald L. Roehrich, December 1980 (original fortran)
  42.  * @author David A. Vallado, Paul Crawford, Richard Hujsak, T.S. Kelso (C++ translation and improvements)
  43.  * @author Fabien Maussion (java translation)
  44.  * @author Thomas Paulet (field translation)
  45.  * @since 11.0
  46.  * @param <T> type of the field elements
  47.  */
  48. public class FieldDeepSDP4<T extends CalculusFieldElement<T>> extends FieldSDP4<T> {

  50.     // CHECKSTYLE: stop JavadocVariable check

  52.     /** Integration step (seconds). */
  53.     private static final double SECULAR_INTEGRATION_STEP  = 720.0;

  55.     /** Intermediate values. */
  56.     private double thgr;
  57.     private T xnq;
  58.     private T omegaq;
  59.     private double zcosil;
  60.     private double zsinil;
  61.     private double zsinhl;
  62.     private double zcoshl;
  63.     private double zmol;
  64.     private double zcosgl;
  65.     private double zsingl;
  66.     private double zmos;
  67.     private T savtsn;

  69.     private T ee2;
  70.     private T e3;
  71.     private T xi2;
  72.     private T xi3;
  73.     private T xl2;
  74.     private T xl3;
  75.     private T xl4;
  76.     private T xgh2;
  77.     private T xgh3;
  78.     private T xgh4;
  79.     private T xh2;
  80.     private T xh3;

  82.     private T d2201;
  83.     private T d2211;
  84.     private T d3210;
  85.     private T d3222;
  86.     private T d4410;
  87.     private T d4422;
  88.     private T d5220;
  89.     private T d5232;
  90.     private T d5421;
  91.     private T d5433;
  92.     private T xlamo;

  94.     private T sse;
  95.     private T ssi;
  96.     private T ssl;
  97.     private T ssh;
  98.     private T ssg;
  99.     private T se2;
  100.     private T si2;
  101.     private T sl2;
  102.     private T sgh2;
  103.     private T sh2;
  104.     private T se3;
  105.     private T si3;
  106.     private T sl3;
  107.     private T sgh3;
  108.     private T sh3;
  109.     private T sl4;
  110.     private T sgh4;

  112.     private T del1;
  113.     private T del2;
  114.     private T del3;
  115.     private T xfact;
  116.     private T xli;
  117.     private T xni;
  118.     private T atime;

  120.     private T pe;
  121.     private T pinc;
  122.     private T pl;
  123.     private T pgh;
  124.     private T ph;

  126.     private T[] derivs;

  128.     // CHECKSTYLE: resume JavadocVariable check

  130.     /** Flag for resonant orbits. */
  131.     private boolean resonant;

  133.     /** Flag for synchronous orbits. */
  134.     private boolean synchronous;

  136.     /** Flag for compliance with Dundee modifications. */
  137.     private boolean isDundeeCompliant = true;

  139.     /** Constructor for a unique initial TLE.
  140.      *
  141.      * <p>This constructor uses the {@link DataContext#getDefault() default data context}.
  142.      *
  143.      * @param initialTLE the TLE to propagate.
  144.      * @param attitudeProvider provider for attitude computation
  145.      * @param mass spacecraft mass (kg)
  146.      * @param parameters SGP4 and SDP4 model parameters
  147.      * @see #FieldDeepSDP4(FieldTLE, AttitudeProvider, CalculusFieldElement, Frame, CalculusFieldElement[])
  148.      */
  149.     @DefaultDataContext
  150.     public FieldDeepSDP4(final FieldTLE<T> initialTLE, final AttitudeProvider attitudeProvider,
  151.                     final T mass, final T[] parameters) {
  152.         this(initialTLE, attitudeProvider, mass,
  153.                 DataContext.getDefault().getFrames().getTEME(), parameters);
  154.     }

  156.     /** Constructor for a unique initial TLE.
  157.      * @param initialTLE the TLE to propagate.
  158.      * @param attitudeProvider provider for attitude computation
  159.      * @param mass spacecraft mass (kg)
  160.      * @param teme the TEME frame to use for propagation.
  161.      * @param parameters SGP4 and SDP4 model parameters
  162.      */
  163.     public FieldDeepSDP4(final FieldTLE<T> initialTLE,
  164.                          final AttitudeProvider attitudeProvider,
  165.                          final T mass,
  166.                          final Frame teme,
  167.                          final T[] parameters) {
  168.         super(initialTLE, attitudeProvider, mass, teme, parameters);
  169.     }

  171.     /** Computes luni - solar terms from initial coordinates and epoch.
  172.      */
  173.     protected void luniSolarTermsComputation() {

  175.         final T zero = tle.getPerigeeArgument().getField().getZero();
  176.         final T pi   = zero.getPi();

  178.         final FieldSinCos<T> scg  = FastMath.sinCos(tle.getPerigeeArgument());
  179.         final T sing = scg.sin();
  180.         final T cosg = scg.cos();

  182.         final FieldSinCos<T> scq  = FastMath.sinCos(tle.getRaan());
  183.         final T sinq = scq.sin();
  184.         final T cosq = scq.cos();
  185.         final T aqnv = a0dp.reciprocal();

  187.         // Compute julian days since 1900
  188.         final double daysSince1900 = (tle.getDate()
  189.                 .getComponents(utc)
  190.                 .offsetFrom(DateTimeComponents.JULIAN_EPOCH)) /
  191.                 Constants.JULIAN_DAY - 2415020;

  193.         double cc = TLEConstants.C1SS;
  194.         double ze = TLEConstants.ZES;
  195.         double zn = TLEConstants.ZNS;
  196.         T zsinh = sinq;
  197.         T zcosh = cosq;

  199.         thgr = thetaG(tle.getDate());
  200.         xnq = xn0dp;
  201.         omegaq = tle.getPerigeeArgument();

  203.         final double xnodce = 4.5236020 - 9.2422029e-4 * daysSince1900;
  204.         final SinCos scTem  = FastMath.sinCos(xnodce);
  205.         final double stem = scTem.sin();
  206.         final double ctem = scTem.cos();
  207.         final double c_minus_gam = 0.228027132 * daysSince1900 - 1.1151842;
  208.         final double gam = 5.8351514 + 0.0019443680 * daysSince1900;

  210.         zcosil = 0.91375164 - 0.03568096 * ctem;
  211.         zsinil = FastMath.sqrt(1.0 - zcosil * zcosil);
  212.         zsinhl = 0.089683511 * stem / zsinil;
  213.         zcoshl = FastMath.sqrt(1.0 - zsinhl * zsinhl);
  214.         zmol = MathUtils.normalizeAngle(c_minus_gam, pi.getReal());

  216.         double zx = 0.39785416 * stem / zsinil;
  217.         final double zy = zcoshl * ctem + 0.91744867 * zsinhl * stem;
  218.         zx = FastMath.atan2( zx, zy) + gam - xnodce;
  219.         final SinCos scZx = FastMath.sinCos(zx);
  220.         zcosgl = scZx.cos();
  221.         zsingl = scZx.sin();
  222.         zmos = MathUtils.normalizeAngle(6.2565837 + 0.017201977 * daysSince1900, pi.getReal());

  224.         // Do solar terms
  225.         savtsn = zero.newInstance(1e20);

  227.         T zcosi = zero.newInstance(0.91744867);
  228.         T zsini = zero.newInstance(0.39785416);
  229.         T zsing = zero.newInstance(-0.98088458);
  230.         T zcosg = zero.newInstance(0.1945905);

  232.         T se =  zero;
  233.         T sgh = zero;
  234.         T sh =  zero;
  235.         T si =  zero;
  236.         T sl =  zero;

  238.         // There was previously some convoluted logic here, but it boils
  239.         // down to this:  we compute the solar terms,  then the lunar terms.
  240.         // On a second pass,  we recompute the solar terms, taking advantage
  241.         // of the improved data that resulted from computing lunar terms.
  242.         for (int iteration = 0; iteration < 2; ++iteration) {
  243.             final T a1  = zcosh.multiply(zcosg).add(zsinh.multiply(zsing).multiply(zcosi));
  244.             final T a3  = zcosh.multiply(zsing.negate()).add(zsinh.multiply(zcosg).multiply(zcosi));
  245.             final T a7  = zsinh.negate().multiply(zcosg).add(zcosh.multiply(zcosi).multiply(zsing));
  246.             final T a8  = zsing.multiply(zsini);
  247.             final T a9  = zsinh.multiply(zsing).add(zcosh.multiply(zcosi).multiply(zcosg));
  248.             final T a10 = zcosg.multiply(zsini);
  249.             final T a2  = cosi0.multiply(a7).add(sini0.multiply(a8));
  250.             final T a4  = cosi0.multiply(a9).add(sini0.multiply(a10));
  251.             final T a5  = sini0.negate().multiply(a7).add(cosi0.multiply(a8));
  252.             final T a6  = sini0.negate().multiply(a9).add(cosi0.multiply(a10));
  253.             final T x1  = a1.multiply(cosg).add(a2.multiply(sing));
  254.             final T x2  = a3.multiply(cosg).add(a4.multiply(sing));
  255.             final T x3  = a1.negate().multiply(sing).add(a2.multiply(cosg));
  256.             final T x4  = a3.negate().multiply(sing).add(a4.multiply(cosg));
  257.             final T x5  = a5.multiply(sing);
  258.             final T x6  = a6.multiply(sing);
  259.             final T x7  = a5.multiply(cosg);
  260.             final T x8  = a6.multiply(cosg);
  261.             final T z31 = x1.square().multiply(12).subtract(x3.square().multiply(3));
  262.             final T z32 = x1.multiply(x2).multiply(24).subtract(x3.multiply(x4).multiply(6));
  263.             final T z33 = x2.square().multiply(12).subtract(x4.square().multiply(3));
  264.             final T z11 = a1.multiply(-6).multiply(a5).add(e0sq.multiply(x1.multiply(x7).multiply(-24).add(x3.multiply(x5).multiply(-6))));
  265.             final T z12 = a1.multiply(a6).add(a3.multiply(a5)).multiply(-6).add(
  266.                                 e0sq.multiply(x2.multiply(x7).add(x1.multiply(x8)).multiply(-24).add(
  267.                                 x3.multiply(x6).add(x4.multiply(x5)).multiply(-6))));
  268.             final T z13 = a3.multiply(a6).multiply(-6).add(e0sq.multiply(
  269.                                x2.multiply(x8).multiply(-24).subtract(x4.multiply(x6).multiply(6))));
  270.             final T z21 = a2.multiply(a5).multiply(6).add(e0sq.multiply(
  271.                                x1.multiply(x5).multiply(24).subtract(x3.multiply(x7).multiply(6))));
  272.             final T z22 = a4.multiply(a5).add(a2.multiply(a6)).multiply(6).add(
  273.                                e0sq.multiply(x2.multiply(x5).add(x1.multiply(x6)).multiply(24).subtract(
  274.                                x4.multiply(x7).add(x3.multiply(x8)).multiply(6))));
  275.             final T z23 = a4.multiply(a6).multiply(6).add(e0sq.multiply(x2.multiply(x6).multiply(24).subtract(x4.multiply(x8).multiply(6))));
  276.             final T s3  = xnq.reciprocal().multiply(cc);
  277.             final T s2  = beta0.reciprocal().multiply(s3.multiply(-0.5));
  278.             final T s4  = s3.multiply(beta0);
  279.             final T s1  = tle.getE().multiply(s4).multiply(-15);
  280.             final T s5  = x1.multiply(x3).add(x2.multiply(x4));
  281.             final T s6  = x2.multiply(x3).add(x1.multiply(x4));
  282.             final T s7  = x2.multiply(x4).subtract(x1.multiply(x3));
  283.             T z1 = a1.square().add(a2.square()).multiply(3).add(z31.multiply(e0sq));
  284.             T z2 = a1.multiply(a3).add(a2.multiply(a4)).multiply(6).add(z32.multiply(e0sq));
  285.             T z3 = a3.square().add(a4.square()).multiply(3).add(z33.multiply(e0sq));

  287.             z1 = z1.add(z1).add(beta02.multiply(z31));
  288.             z2 = z2.add(z2).add(beta02.multiply(z32));
  289.             z3 = z3.add(z3).add(beta02.multiply(z33));
  290.             se = s1.multiply(zn).multiply(s5);
  291.             si = s2.multiply(zn).multiply(z11.add(z13));
  292.             sl = s3.multiply(-zn).multiply(z1.add(z3).subtract(14).subtract(e0sq.multiply(6)));
  293.             sgh = s4.multiply(zn).multiply(z31.add(z33).subtract(6));
  294.             if (tle.getI().getReal() < pi.divide(60.0).getReal()) {
  295.                 // inclination smaller than 3 degrees
  296.                 sh = zero;
  297.             } else {
  298.                 sh = s2.multiply(-zn).multiply(z21.add(z23));
  299.             }
  300.             ee2  = s1.multiply(s6).multiply(2);
  301.             e3   = s1.multiply(s7).multiply(2);
  302.             xi2  = s2.multiply(z12).multiply(2);
  303.             xi3  = s2.multiply(z13.subtract(z11)).multiply(2);
  304.             xl2  = s3.multiply(z2).multiply(-2);
  305.             xl3  = s3.multiply(z3.subtract(z1)).multiply(-2);
  306.             xl4  = s3.multiply(e0sq.multiply(-9).add(-21)).multiply(ze).multiply(-2);
  307.             xgh2 = s4.multiply(z32).multiply(2);
  308.             xgh3 = s4.multiply(z33.subtract(z31)).multiply(2);
  309.             xgh4 = s4.multiply(ze).multiply(-18);
  310.             xh2  = s2.multiply(z22).multiply(-2);
  311.             xh3  = s2.multiply(z23.subtract(z21)).multiply(-2);

  313.             if (iteration == 0) { // we compute lunar terms only on the first pass:
  314.                 sse = se;
  315.                 ssi = si;
  316.                 ssl = sl;
  317.                 ssh = (tle.getI().getReal() < pi.divide(60.0).getReal()) ? zero : sh.divide(sini0);
  318.                 ssg = sgh.subtract(cosi0.multiply(ssh));
  319.                 se2 = ee2;
  320.                 si2 = xi2;
  321.                 sl2 = xl2;
  322.                 sgh2 = xgh2;
  323.                 sh2 = xh2;
  324.                 se3 = e3;
  325.                 si3 = xi3;
  326.                 sl3 = xl3;
  327.                 sgh3 = xgh3;
  328.                 sh3 = xh3;
  329.                 sl4 = xl4;
  330.                 sgh4 = xgh4;
  331.                 zcosg = zero.newInstance(zcosgl);
  332.                 zsing = zero.newInstance(zsingl);
  333.                 zcosi = zero.newInstance(zcosil);
  334.                 zsini = zero.newInstance(zsinil);
  335.                 zcosh = cosq.multiply(zcoshl).add(sinq.multiply(zsinhl));
  336.                 zsinh = sinq.multiply(zcoshl).subtract(cosq.multiply(zsinhl));
  337.                 zn = TLEConstants.ZNL;
  338.                 cc = TLEConstants.C1L;
  339.                 ze = TLEConstants.ZEL;
  340.             }
  341.         } // end of solar - lunar - solar terms computation

  343.         sse = sse.add(se);
  344.         ssi = ssi.add(si);
  345.         ssl = ssl.add(sl);
  346.         ssg = ssg.add(sgh).subtract((tle.getI().getReal() < pi.divide(60.0).getReal()) ? zero : (cosi0.divide(sini0).multiply(sh)));
  347.         ssh = ssh.add((tle.getI().getReal() < pi.divide(60.0).getReal()) ? zero : sh.divide(sini0));



  351.         //        Start the resonant-synchronous tests and initialization

  353.         T bfact = zero;

  355.         // if mean motion is 1.893053 to 2.117652 revs/day, and eccentricity >= 0.5,
  356.         // start of the 12-hour orbit, e > 0.5 section
  357.         if (xnq.getReal() >= 0.00826 && xnq.getReal() <= 0.00924 && tle.getE().getReal() >= 0.5) {

  359.             final T g201  = tle.getE().subtract(0.64).negate().multiply(0.440).add(-0.306);
  360.             final T eoc   = tle.getE().multiply(e0sq);
  361.             final T sini2 = sini0.multiply(sini0);
  362.             final T f220  = cosi0.multiply(2).add(theta2).add(1).multiply(0.75);
  363.             final T f221  = sini2.multiply(1.5);
  364.             final T f321  = sini0.multiply(1.875).multiply(cosi0.multiply(2).negate().subtract(theta2.multiply(3)).add(1));
  365.             final T f322  = sini0.multiply(-1.875).multiply(cosi0.multiply(2).subtract(theta2.multiply(3)).add(1));
  366.             final T f441  = sini2.multiply(f220).multiply(35);
  367.             final T f442  = sini2.multiply(sini2).multiply(39.3750);
  368.             final T f522  = sini0.multiply(9.84375).multiply(sini2.multiply(cosi0.multiply(-2).add(theta2.multiply(-5)).add(1.0)).add(
  369.                                     cosi0.multiply(4.0).add(theta2.multiply(6.0)).add(-2).multiply(0.33333333)));
  370.             final T f523  = sini0.multiply(sini2.multiply(cosi0.multiply(-4).add(theta2.multiply(10)).add(-2)).multiply(4.92187512).add(
  371.                                     cosi0.multiply(2).subtract(theta2.multiply(3)).add(1).multiply(6.56250012)));
  372.             final T f542  = sini0.multiply(29.53125).multiply(cosi0.multiply(-8).add(2).add(
  373.                                     theta2.multiply(cosi0.multiply(8).add(theta2.multiply(10)).add(-12))));
  374.             final T f543  = sini0.multiply(29.53125).multiply(cosi0.multiply(-8).add(-2).add(
  375.                                     theta2.multiply(cosi0.multiply(8).subtract(theta2.multiply(10)).add(12))));
  376.             final T g211;
  377.             final T g310;
  378.             final T g322;
  379.             final T g410;
  380.             final T g422;
  381.             final T g520;

  383.             resonant = true;       // it is resonant...
  384.             synchronous = false;     // but it's not synchronous

  386.             // Geopotential resonance initialization for 12 hour orbits :
  387.             if (tle.getE().getReal() <= 0.65) {
  388.                 g211 = tle.getE().multiply( -13.247).add(  e0sq.multiply(   16.290)).add(                                  3.616);
  389.                 g310 = tle.getE().multiply( 117.390).add(  e0sq.multiply( -228.419)).add(  eoc.multiply( 156.591)).add(  -19.302);
  390.                 g322 = tle.getE().multiply(109.7927).add(  e0sq.multiply(-214.6334)).add(  eoc.multiply(146.5816)).add( -18.9068);
  391.                 g410 = tle.getE().multiply( 242.694).add(  e0sq.multiply( -471.094)).add(  eoc.multiply( 313.953)).add(  -41.122);
  392.                 g422 = tle.getE().multiply( 841.880).add(  e0sq.multiply(-1629.014)).add(  eoc.multiply(1083.435)).add( -146.407);
  393.                 g520 = tle.getE().multiply(3017.977).add(  e0sq.multiply(-5740.032)).add(  eoc.multiply(3708.276)).add( -532.114);
  394.             } else  {
  395.                 g211 = tle.getE().multiply( 331.819).add(  e0sq.multiply( -508.738)).add(  eoc.multiply( 266.724)).add(  -72.099);
  396.                 g310 = tle.getE().multiply(1582.851).add(  e0sq.multiply(-2415.925)).add(  eoc.multiply(1246.113)).add( -346.844);
  397.                 g322 = tle.getE().multiply(1554.908).add(  e0sq.multiply(-2366.899)).add(  eoc.multiply(1215.972)).add( -342.585);
  398.                 g410 = tle.getE().multiply(4758.686).add(  e0sq.multiply(-7193.992)).add(  eoc.multiply(3651.957)).add(-1052.797);
  399.                 g422 = tle.getE().multiply(16178.11).add(  e0sq.multiply(-24462.77)).add(  eoc.multiply(12422.52)).add( -3581.69);
  400.                 if (tle.getE().getReal() <= 0.715) {
  401.                     g520 = tle.getE().multiply(-4664.75).add(  e0sq.multiply(  3763.64)).add(                                1464.74);
  402.                 } else {
  403.                     g520 = tle.getE().multiply(29936.92).add(  e0sq.multiply(-54087.36)).add(  eoc.multiply(31324.56)).add( -5149.66);
  404.                 }
  405.             }

  407.             final T g533;
  408.             final T g521;
  409.             final T g532;
  410.             if (tle.getE().getReal() < 0.7) {
  411.                 g533 = tle.getE().multiply(  4988.61).add(  e0sq.multiply(  -9064.77)).add(  eoc.multiply(  5542.21)).add(  -919.2277);
  412.                 g521 = tle.getE().multiply(4568.6173).add(  e0sq.multiply(-8491.4146)).add(  eoc.multiply( 5337.524)).add( -822.71072);
  413.                 g532 = tle.getE().multiply(  4690.25).add(  e0sq.multiply(  -8624.77)).add(  eoc.multiply(   5341.4)).add(   -853.666);
  414.             } else {
  415.                 g533 = tle.getE().multiply(161616.52).add(  e0sq.multiply( -229838.2)).add(  eoc.multiply(109377.94)).add(  -37995.78);
  416.                 g521 = tle.getE().multiply(218913.95).add(  e0sq.multiply(-309468.16)).add(  eoc.multiply(146349.42)).add( -51752.104);
  417.                 g532 = tle.getE().multiply(170470.89).add(  e0sq.multiply(-242699.48)).add(  eoc.multiply(115605.82)).add(  -40023.88);
  418.             }

  420.             T temp1 = xnq.multiply(xnq).multiply(aqnv).multiply(aqnv).multiply(3);
  421.             T temp  = temp1.multiply(TLEConstants.ROOT22);
  422.             d2201   = temp.multiply(f220).multiply(g201);
  423.             d2211   = temp.multiply(f221).multiply(g211);
  424.             temp1   = temp1.multiply(aqnv);
  425.             temp    = temp1.multiply(TLEConstants.ROOT32);
  426.             d3210   = temp.multiply(f321).multiply(g310);
  427.             d3222   = temp.multiply(f322).multiply(g322);
  428.             temp1   = temp1.multiply(aqnv);
  429.             temp    = temp1.multiply(2 * TLEConstants.ROOT44);
  430.             d4410   = temp.multiply(f441).multiply(g410);
  431.             d4422   = temp.multiply(f442).multiply(g422);
  432.             temp1   = temp1.multiply(aqnv);
  433.             temp    = temp1.multiply(TLEConstants.ROOT52);
  434.             d5220   = temp.multiply(f522).multiply(g520);
  435.             d5232   = temp.multiply(f523).multiply(g532);
  436.             temp    = temp1.multiply(2 * TLEConstants.ROOT54);
  437.             d5421   = temp.multiply(f542).multiply(g521);
  438.             d5433   = temp.multiply(f543).multiply(g533);
  439.             xlamo   = tle.getMeanAnomaly().add(tle.getRaan()).add(tle.getRaan()).subtract(thgr + thgr);
  440.             bfact   = xmdot.add(xnodot).add(xnodot).subtract(TLEConstants.THDT + TLEConstants.THDT);
  441.             bfact   = bfact.add(ssl).add(ssh).add(ssh);
  442.         } else if (xnq.getReal() < 0.0052359877 && xnq.getReal() > 0.0034906585) {
  443.             // if mean motion is .8 to 1.2 revs/day : (geosynch)

  445.             final T cosio_plus_1 = cosi0.add(1.0);
  446.             final T g200 = e0sq.multiply(e0sq.multiply(0.8125).add(-2.5)).add(1);
  447.             final T g300 = e0sq.multiply(e0sq.multiply(6.60937).add(-6)).add(1);
  448.             final T f311 = sini0.multiply(0.9375).multiply(sini0.multiply(cosi0.multiply(3).add(1))).subtract(cosio_plus_1.multiply(0.75));
  449.             final T g310 = e0sq.multiply(2).add(1);
  450.             final T f220 = cosio_plus_1.multiply(cosio_plus_1).multiply(0.75);
  451.             final T f330 = f220.multiply(cosio_plus_1).multiply(2.5);

  453.             resonant = true;
  454.             synchronous = true;

  456.             // Synchronous resonance terms initialization
  457.             del1 = xnq.multiply(xnq).multiply(aqnv).multiply(aqnv).multiply(3);
  458.             del2 = del1.multiply(f220).multiply(g200).multiply(2 * TLEConstants.Q22);
  459.             del3 = del1.multiply(f330).multiply(g300).multiply(aqnv).multiply(3 * TLEConstants.Q33);
  460.             del1 = del1.multiply(f311).multiply(g310).multiply(TLEConstants.Q31).multiply(aqnv);
  461.             xlamo = tle.getMeanAnomaly().add(tle.getRaan()).add(tle.getPerigeeArgument()).subtract(thgr);
  462.             bfact = xmdot.add(omgdot).add(xnodot).subtract(TLEConstants.THDT);
  463.             bfact = bfact.add(ssl).add(ssg).add(ssh);
  464.         } else {
  465.             // it's neither a high-e 12-hours orbit nor a geosynchronous:
  466.             resonant = false;
  467.             synchronous = false;
  468.         }

  470.         if (resonant) {
  471.             xfact = bfact.subtract(xnq);

  473.             // Initialize integrator
  474.             xli   = xlamo;
  475.             xni   = xnq;
  476.             atime = zero;
  477.         }
  478.         derivs = MathArrays.buildArray(xnq.getField(), 2);
  479.     }

  481.     /** Computes secular terms from current coordinates and epoch.
  482.      * @param t offset from initial epoch (minutes)
  483.      */
  484.     protected void deepSecularEffects(final T t)  {

  486.         xll     = xll.add(ssl.multiply(t));
  487.         omgadf  = omgadf.add(ssg.multiply(t));
  488.         xnode   = xnode.add(ssh.multiply(t));
  489.         em      = tle.getE().add(sse.multiply(t));
  490.         xinc    = tle.getI().add(ssi.multiply(t));

  492.         if (resonant) {
  493.             // If we're closer to t = 0 than to the currently-stored data
  494.             // from the previous call to this function,  then we're
  495.             // better off "restarting",  going back to the initial data.
  496.             // The Dundee code rigs things up to _always_ take 720-minute
  497.             // steps from epoch to end time,  except for the final step.
  498.             // Easiest way to arrange similar behavior in this code is
  499.             // just to always do a restart,  if we're in Dundee-compliant
  500.             // mode.
  501.             if (FastMath.abs(t).getReal() < FastMath.abs(t.subtract(atime)).getReal() || isDundeeCompliant)  {
  502.                 // Epoch restart
  503.                 atime = t.getField().getZero();
  504.                 xni = xnq;
  505.                 xli = xlamo;
  506.             }
  507.             boolean lastIntegrationStep = false;
  508.             // if |step|>|step max| then do one step at step max
  509.             while (!lastIntegrationStep) {
  510.                 double delt = t.subtract(atime).getReal();
  511.                 if (delt > SECULAR_INTEGRATION_STEP) {
  512.                     delt = SECULAR_INTEGRATION_STEP;
  513.                 } else if (delt < -SECULAR_INTEGRATION_STEP) {
  514.                     delt = -SECULAR_INTEGRATION_STEP;
  515.                 } else {
  516.                     lastIntegrationStep = true;
  517.                 }

  519.                 computeSecularDerivs();

  521.                 final T xldot = xni.add(xfact);

  523.                 T xlpow = t.getField().getOne();
  524.                 xli = xli.add(xldot.multiply(delt));
  525.                 xni = xni.add(derivs[0].multiply(delt));
  526.                 double delt_factor = delt;
  527.                 xlpow = xlpow.multiply(xldot);
  528.                 derivs[1] = derivs[1].multiply(xlpow);
  529.                 delt_factor *= delt / 2;
  530.                 xli = xli.add(derivs[0].multiply(delt_factor));
  531.                 xni = xni.add(derivs[1].multiply(delt_factor));
  532.                 atime = atime.add(delt);
  533.             }
  534.             xn = xni;
  535.             final T temp = xnode.negate().add(thgr).add(t.multiply(TLEConstants.THDT));
  536.             xll = xli.add(temp).add(synchronous ? omgadf.negate() : temp);
  537.         }
  538.     }

  540.     /** Computes periodic terms from current coordinates and epoch.
  541.      * @param t offset from initial epoch (min)
  542.      */
  543.     protected void deepPeriodicEffects(final T t)  {

  545.         // If the time didn't change by more than 30 minutes,
  546.         // there's no good reason to recompute the perturbations;
  547.         // they don't change enough over so short a time span.
  548.         // However,  the Dundee code _always_ recomputes,  so if
  549.         // we're attempting to replicate its results,  we've gotta
  550.         // recompute everything,  too.
  551.         if (FastMath.abs(savtsn.subtract(t).getReal()) >= 30.0 || isDundeeCompliant)  {

  553.             savtsn = t;

  555.             // Update solar perturbations for time T
  556.             T zm = t.multiply(TLEConstants.ZNS).add(zmos);
  557.             T zf = zm.add(FastMath.sin(zm).multiply(2 * TLEConstants.ZES));
  558.             FieldSinCos<T> sczf = FastMath.sinCos(zf);
  559.             T sinzf = sczf.sin();
  560.             T f2 = sinzf.multiply(sinzf).multiply(0.5).subtract(0.25);
  561.             T f3 = sinzf.multiply(sczf.cos()).multiply(-0.5);
  562.             final T ses = se2.multiply(f2).add(se3.multiply(f3));
  563.             final T sis = si2.multiply(f2).add(si3.multiply(f3));
  564.             final T sls = sl2.multiply(f2).add(sl3.multiply(f3)).add(sl4.multiply(sinzf));
  565.             final T sghs = sgh2.multiply(f2).add(sgh3.multiply(f3)).add(sgh4.multiply(sinzf));
  566.             final T shs = sh2.multiply(f2).add(sh3.multiply(f3));

  568.             // Update lunar perturbations for time T
  569.             zm = t.multiply(TLEConstants.ZNL).add(zmol);
  570.             zf = zm.add(FastMath.sin(zm).multiply(2 * TLEConstants.ZEL));
  571.             sczf = FastMath.sinCos(zf);
  572.             sinzf = sczf.sin();
  573.             f2 =  sinzf.multiply(sinzf).multiply(0.5).subtract(0.25);
  574.             f3 = sinzf.multiply(sczf.cos()).multiply(-0.5);
  575.             final T sel = ee2.multiply(f2).add(e3.multiply(f3));
  576.             final T sil = xi2.multiply(f2).add(xi3.multiply(f3));
  577.             final T sll = xl2.multiply(f2).add(xl3.multiply(f3)).add(xl4.multiply(sinzf));
  578.             final T sghl = xgh2.multiply(f2).add(xgh3.multiply(f3)).add(xgh4.multiply(sinzf));
  579.             final T sh1 = xh2.multiply(f2).add(xh3.multiply(f3));

  581.             // Sum the solar and lunar contributions
  582.             pe   = ses.add(sel);
  583.             pinc = sis.add(sil);
  584.             pl   = sls.add(sll);
  585.             pgh  = sghs.add(sghl);
  586.             ph   = shs.add(sh1);
  587.         }

  589.         xinc = xinc.add(pinc);

  591.         final FieldSinCos<T> scis = FastMath.sinCos(xinc);
  592.         final T sinis = scis.sin();
  593.         final T cosis = scis.cos();

  595.         /* Add solar/lunar perturbation correction to eccentricity: */
  596.         em     = em.add(pe);
  597.         xll    = xll.add(pl);
  598.         omgadf = omgadf.add(pgh);
  599.         xinc   = MathUtils.normalizeAngle(xinc, t.getField().getZero());

  601.         if (FastMath.abs(xinc).getReal() >= 0.2) {
  602.             // Apply periodics directly
  603.             final T temp_val = ph.divide(sinis);
  604.             omgadf = omgadf.subtract(cosis.multiply(temp_val));
  605.             xnode  = xnode.add(temp_val);
  606.         } else {
  607.             // Apply periodics with Lyddane modification
  608.             final FieldSinCos<T> scok = FastMath.sinCos(xnode);
  609.             final T sinok = scok.sin();
  610.             final T cosok = scok.cos();
  611.             final T alfdp =  ph.multiply(cosok).add((pinc.multiply(cosis).add(sinis)).multiply(sinok));
  612.             final T betdp = ph.negate().multiply(sinok).add((pinc.multiply(cosis).add(sinis)).multiply(cosok));
  613.             final T delta_xnode = MathUtils.normalizeAngle(FastMath.atan2(alfdp, betdp).subtract(xnode), t.getField().getZero());
  614.             final T dls = xnode.negate().multiply(sinis).multiply(pinc);
  615.             omgadf = omgadf.add(dls.subtract(cosis.multiply(delta_xnode)));
  616.             xnode  = xnode.add(delta_xnode);
  617.         }
  618.     }

  620.     /** Computes internal secular derivs. */
  621.     private void computeSecularDerivs() {

  623.         final FieldSinCos<T> sc_li  = FastMath.sinCos(xli);
  624.         final T sin_li = sc_li.sin();
  625.         final T cos_li = sc_li.cos();
  626.         final T sin_2li = sin_li.multiply(cos_li).multiply(2.);
  627.         final T cos_2li = cos_li.multiply(cos_li).multiply(2.).subtract(1.);

  629.         // Dot terms calculated :
  630.         if (synchronous)  {
  631.             final T sin_3li = sin_2li.multiply(cos_li).add(cos_2li.multiply(sin_li));
  632.             final T cos_3li = cos_2li.multiply(cos_li).subtract(sin_2li.multiply(sin_li));
  633.             final T term1a = del1.multiply(sin_li .multiply(TLEConstants.C_FASX2) .subtract(cos_li .multiply(TLEConstants.S_FASX2 )));
  634.             final T term2a = del2.multiply(sin_2li.multiply(TLEConstants.C_2FASX4).subtract(cos_2li.multiply(TLEConstants.S_2FASX4)));
  635.             final T term3a = del3.multiply(sin_3li.multiply(TLEConstants.C_3FASX6).subtract(cos_3li.multiply(TLEConstants.S_3FASX6)));
  636.             final T term1b = del1.multiply(cos_li .multiply(TLEConstants.C_FASX2)      .add(sin_li .multiply(TLEConstants.S_FASX2 )));
  637.             final T term2b = del2.multiply(cos_2li.multiply(TLEConstants.C_2FASX4)     .add(sin_2li.multiply(TLEConstants.S_2FASX4))).multiply(2.0);
  638.             final T term3b = del3.multiply(cos_3li.multiply(TLEConstants.C_3FASX6)     .add(sin_3li.multiply(TLEConstants.S_3FASX6))).multiply(3.0);
  639.             derivs[0] = term1a.add(term2a).add(term3a);
  640.             derivs[1] = term1b.add(term2b).add(term3b);
  641.         } else {
  642.             // orbit is a 12-hour resonant one
  643.             final T xomi = omegaq.add(omgdot.multiply(atime));
  644.             final FieldSinCos<T> sc_omi  = FastMath.sinCos(xomi);
  645.             final T sin_omi = sc_omi.sin();
  646.             final T cos_omi = sc_omi.cos();
  647.             final T sin_li_m_omi = sin_li.multiply(cos_omi).subtract(sin_omi.multiply(cos_li));
  648.             final T sin_li_p_omi = sin_li.multiply(cos_omi).add(     sin_omi.multiply(cos_li));
  649.             final T cos_li_m_omi = cos_li.multiply(cos_omi).add(     sin_omi.multiply(sin_li));
  650.             final T cos_li_p_omi = cos_li.multiply(cos_omi).subtract(sin_omi.multiply(sin_li));
  651.             final T sin_2omi = sin_omi.multiply(cos_omi).multiply(2.0);
  652.             final T cos_2omi = cos_omi.multiply(cos_omi).multiply(2.0).subtract(1.0);
  653.             final T sin_2li_m_omi  = sin_2li.multiply(cos_omi ).subtract(sin_omi .multiply(cos_2li));
  654.             final T sin_2li_p_omi  = sin_2li.multiply(cos_omi ).add(     sin_omi .multiply(cos_2li));
  655.             final T cos_2li_m_omi  = cos_2li.multiply(cos_omi ).add(     sin_omi .multiply(sin_2li));
  656.             final T cos_2li_p_omi  = cos_2li.multiply(cos_omi ).subtract(sin_omi .multiply(sin_2li));
  657.             final T sin_2li_p_2omi = sin_2li.multiply(cos_2omi).add(     sin_2omi.multiply(cos_2li));
  658.             final T cos_2li_p_2omi = cos_2li.multiply(cos_2omi).subtract(sin_2omi.multiply(sin_2li));
  659.             final T sin_2omi_p_li  = sin_li .multiply(cos_2omi).add(     sin_2omi.multiply(cos_li ));
  660.             final T cos_2omi_p_li  = cos_li .multiply(cos_2omi).subtract(sin_2omi.multiply(sin_li ));
  661.             final T term1a = d2201.multiply(sin_2omi_p_li .multiply(TLEConstants.C_G22).subtract(cos_2omi_p_li .multiply(TLEConstants.S_G22))) .add(
  662.                              d2211.multiply(sin_li        .multiply(TLEConstants.C_G22).subtract(cos_li        .multiply(TLEConstants.S_G22)))).add(
  663.                              d3210.multiply(sin_li_p_omi  .multiply(TLEConstants.C_G32).subtract(cos_li_p_omi  .multiply(TLEConstants.S_G32)))).add(
  664.                              d3222.multiply(sin_li_m_omi  .multiply(TLEConstants.C_G32).subtract(cos_li_m_omi  .multiply(TLEConstants.S_G32)))).add(
  665.                              d5220.multiply(sin_li_p_omi  .multiply(TLEConstants.C_G52).subtract(cos_li_p_omi  .multiply(TLEConstants.S_G52)))).add(
  666.                              d5232.multiply(sin_li_m_omi  .multiply(TLEConstants.C_G52).subtract(cos_li_m_omi  .multiply(TLEConstants.S_G52))));
  667.             final T term2a = d4410.multiply(sin_2li_p_2omi.multiply(TLEConstants.C_G44).subtract(cos_2li_p_2omi.multiply(TLEConstants.S_G44))) .add(
  668.                              d4422.multiply(sin_2li       .multiply(TLEConstants.C_G44).subtract(cos_2li       .multiply(TLEConstants.S_G44)))).add(
  669.                              d5421.multiply(sin_2li_p_omi .multiply(TLEConstants.C_G54).subtract(cos_2li_p_omi .multiply(TLEConstants.S_G54)))).add(
  670.                              d5433.multiply(sin_2li_m_omi .multiply(TLEConstants.C_G54).subtract(cos_2li_m_omi .multiply(TLEConstants.S_G54))));
  671.             final T term1b = d2201.multiply(cos_2omi_p_li .multiply(TLEConstants.C_G22)     .add(sin_2omi_p_li .multiply(TLEConstants.S_G22))) .add(
  672.                              d2211.multiply(cos_li        .multiply(TLEConstants.C_G22)     .add(sin_li        .multiply(TLEConstants.S_G22)))).add(
  673.                              d3210.multiply(cos_li_p_omi  .multiply(TLEConstants.C_G32)     .add(sin_li_p_omi  .multiply(TLEConstants.S_G32)))).add(
  674.                              d3222.multiply(cos_li_m_omi  .multiply(TLEConstants.C_G32)     .add(sin_li_m_omi  .multiply(TLEConstants.S_G32)))).add(
  675.                              d5220.multiply(cos_li_p_omi  .multiply(TLEConstants.C_G52)     .add(sin_li_p_omi  .multiply(TLEConstants.S_G52)))).add(
  676.                              d5232.multiply(cos_li_m_omi  .multiply(TLEConstants.C_G52)     .add(sin_li_m_omi  .multiply(TLEConstants.S_G52))));
  677.             final T term2b = d4410.multiply(cos_2li_p_2omi.multiply(TLEConstants.C_G44)     .add(sin_2li_p_2omi.multiply(TLEConstants.S_G44))) .add(
  678.                              d4422.multiply(cos_2li       .multiply(TLEConstants.C_G44)     .add(sin_2li       .multiply(TLEConstants.S_G44)))).add(
  679.                              d5421.multiply(cos_2li_p_omi .multiply(TLEConstants.C_G54)     .add(sin_2li_p_omi .multiply(TLEConstants.S_G54)))).add(
  680.                              d5433.multiply(cos_2li_m_omi .multiply(TLEConstants.C_G54)     .add(sin_2li_m_omi .multiply(TLEConstants.S_G54)))).multiply(2.0);

  682.             derivs[0] = term1a.add(term2a);
  683.             derivs[1] = term1b.add(term2b);

  685.         }
  686.     }

  688. }
Created with JaCoCo 0.8.12.202403310830