FieldDeepSDP4.java

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

  49.     // CHECKSTYLE: stop JavadocVariable check

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

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

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

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

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

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

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

  125.     private T[] derivs;

  127.     // CHECKSTYLE: resume JavadocVariable check

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  223.         // Do solar terms
  224.         savtsn = zero.add(1e20);

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

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

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

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

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

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



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

  352.         T bfact = zero;

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

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

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

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

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

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

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

  452.             resonant = true;
  453.             synchronous = true;

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

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

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

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

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

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

  518.                 computeSecularDerivs();

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

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

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

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

  552.             savtsn = t;

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

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

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

  588.         xinc = xinc.add(pinc);

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

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

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

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

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

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

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

  684.         }
  685.     }

  687. }
Created with JaCoCo 0.8.8.202204050719