OrbitType.java

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

  19. import java.util.Arrays;

  21. import org.hipparchus.RealFieldElement;
  22. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  23. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  24. import org.hipparchus.util.FastMath;
  25. import org.orekit.errors.OrekitException;
  26. import org.orekit.errors.OrekitMessages;
  27. import org.orekit.frames.Frame;
  28. import org.orekit.time.AbsoluteDate;
  29. import org.orekit.time.FieldAbsoluteDate;
  30. import org.orekit.utils.FieldPVCoordinates;
  31. import org.orekit.utils.PVCoordinates;
  32. import org.orekit.utils.ParameterDriver;
  33. import org.orekit.utils.ParameterDriversList;
  34. import org.orekit.utils.TimeStampedFieldPVCoordinates;

  36. /** Enumerate for {@link Orbit orbital} parameters types.
  37.  */
  38. public enum OrbitType {

  40.     /** Type for propagation in {@link CartesianOrbit Cartesian parameters}. */
  41.     CARTESIAN {

  43.         /** {@inheritDoc} */
  44.         @Override
  45.         public CartesianOrbit convertType(final Orbit orbit) {
  46.             return (orbit.getType() == this) ? (CartesianOrbit) orbit : new CartesianOrbit(orbit);
  47.         }

  49.         /** {@inheritDoc} */
  50.         @Override
  51.         public void mapOrbitToArray(final Orbit orbit, final PositionAngle type,
  52.                                     final double[] stateVector, final double[] stateVectorDot) {

  54.             final PVCoordinates pv = orbit.getPVCoordinates();
  55.             final Vector3D      p  = pv.getPosition();
  56.             final Vector3D      v  = pv.getVelocity();

  58.             stateVector[0] = p.getX();
  59.             stateVector[1] = p.getY();
  60.             stateVector[2] = p.getZ();
  61.             stateVector[3] = v.getX();
  62.             stateVector[4] = v.getY();
  63.             stateVector[5] = v.getZ();

  65.             if (stateVectorDot != null) {
  66.                 final Vector3D a  = pv.getAcceleration();
  67.                 stateVectorDot[0] = v.getX();
  68.                 stateVectorDot[1] = v.getY();
  69.                 stateVectorDot[2] = v.getZ();
  70.                 stateVectorDot[3] = a.getX();
  71.                 stateVectorDot[4] = a.getY();
  72.                 stateVectorDot[5] = a.getZ();
  73.             }

  75.         }

  77.         /** {@inheritDoc} */
  78.         @Override
  79.         public CartesianOrbit mapArrayToOrbit(final double[] stateVector, final double[] stateVectorDot, final PositionAngle type,
  80.                                               final AbsoluteDate date, final double mu, final Frame frame) {

  82.             final Vector3D p = new Vector3D(stateVector[0], stateVector[1], stateVector[2]);
  83.             final Vector3D v = new Vector3D(stateVector[3], stateVector[4], stateVector[5]);
  84.             final Vector3D a;
  85.             if (stateVectorDot == null) {
  86.                 // we don't have data about acceleration
  87.                 return new CartesianOrbit(new PVCoordinates(p, v), frame, date, mu);
  88.             } else {
  89.                 // we do have an acceleration
  90.                 a = new Vector3D(stateVectorDot[3], stateVectorDot[4], stateVectorDot[5]);
  91.                 return new CartesianOrbit(new PVCoordinates(p, v, a), frame, date, mu);
  92.             }

  94.         }

  96.         /** {@inheritDoc} */
  97.         @Override
  98.         public <T extends RealFieldElement<T>> FieldCartesianOrbit<T> convertType(final FieldOrbit<T> orbit) {
  99.             return (orbit.getType() == this) ? (FieldCartesianOrbit<T>) orbit : new FieldCartesianOrbit<>(orbit);
  100.         }

  102.         /** {@inheritDoc} */
  103.         @Override
  104.         public <T extends RealFieldElement<T>> void mapOrbitToArray(final FieldOrbit<T> orbit,
  105.                                                                     final PositionAngle type,
  106.                                                                     final T[] stateVector,
  107.                                                                     final T[] stateVectorDot) {

  109.             final TimeStampedFieldPVCoordinates<T> pv = orbit.getPVCoordinates();
  110.             final FieldVector3D<T>                 p  = pv.getPosition();
  111.             final FieldVector3D<T>                 v  = pv.getVelocity();

  113.             stateVector[0] = p.getX();
  114.             stateVector[1] = p.getY();
  115.             stateVector[2] = p.getZ();
  116.             stateVector[3] = v.getX();
  117.             stateVector[4] = v.getY();
  118.             stateVector[5] = v.getZ();

  120.             if (stateVectorDot != null) {
  121.                 final FieldVector3D<T> a = pv.getAcceleration();
  122.                 stateVectorDot[0] = v.getX();
  123.                 stateVectorDot[1] = v.getY();
  124.                 stateVectorDot[2] = v.getZ();
  125.                 stateVectorDot[3] = a.getX();
  126.                 stateVectorDot[4] = a.getY();
  127.                 stateVectorDot[5] = a.getZ();
  128.             }

  130.         }

  132.         /** {@inheritDoc} */
  133.         @Override
  134.         public <T extends RealFieldElement<T>> FieldCartesianOrbit<T> mapArrayToOrbit(final T[] stateVector,
  135.                                                                                       final T[] stateVectorDot,
  136.                                                                                       final PositionAngle type,
  137.                                                                                       final FieldAbsoluteDate<T> date,
  138.                                                                                       final T mu, final Frame frame) {
  139.             final FieldVector3D<T> p = new FieldVector3D<>(stateVector[0], stateVector[1], stateVector[2]);
  140.             final FieldVector3D<T> v = new FieldVector3D<>(stateVector[3], stateVector[4], stateVector[5]);
  141.             final FieldVector3D<T> a;
  142.             if (stateVectorDot == null) {
  143.                 // we don't have data about acceleration
  144.                 return new FieldCartesianOrbit<>(new FieldPVCoordinates<>(p, v), frame, date, mu);
  145.             } else {
  146.                 // we do have an acceleration
  147.                 a = new FieldVector3D<>(stateVectorDot[3], stateVectorDot[4], stateVectorDot[5]);
  148.                 return new FieldCartesianOrbit<>(new FieldPVCoordinates<>(p, v, a), frame, date, mu);
  149.             }

  151.         }

  153.         /** {@inheritDoc} */
  154.         @Override
  155.         public ParameterDriversList getDrivers(final double dP, final Orbit orbit, final PositionAngle type) {
  156.             final ParameterDriversList drivers = new ParameterDriversList();
  157.             final double[] array = new double[6];
  158.             mapOrbitToArray(orbit, type, array, null);
  159.             final double[] scale = scale(dP, orbit);
  160.             drivers.add(new ParameterDriver(POS_X, array[0], scale[0], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  161.             drivers.add(new ParameterDriver(POS_Y, array[1], scale[1], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  162.             drivers.add(new ParameterDriver(POS_Z, array[2], scale[2], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  163.             drivers.add(new ParameterDriver(VEL_X, array[3], scale[3], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  164.             drivers.add(new ParameterDriver(VEL_Y, array[4], scale[4], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  165.             drivers.add(new ParameterDriver(VEL_Z, array[5], scale[5], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  166.             return drivers;
  167.         }

  169.     },

  171.     /** Type for propagation in {@link CircularOrbit circular parameters}. */
  172.     CIRCULAR {

  174.         /** {@inheritDoc} */
  175.         @Override
  176.         public CircularOrbit convertType(final Orbit orbit) {
  177.             return (orbit.getType() == this) ? (CircularOrbit) orbit : new CircularOrbit(orbit);
  178.         }

  180.         /** {@inheritDoc} */
  181.         @Override
  182.         public void mapOrbitToArray(final Orbit orbit, final PositionAngle type,
  183.                                     final double[] stateVector, final double[] stateVectorDot) {

  185.             final CircularOrbit circularOrbit = (CircularOrbit) OrbitType.CIRCULAR.convertType(orbit);

  187.             stateVector[0] = circularOrbit.getA();
  188.             stateVector[1] = circularOrbit.getCircularEx();
  189.             stateVector[2] = circularOrbit.getCircularEy();
  190.             stateVector[3] = circularOrbit.getI();
  191.             stateVector[4] = circularOrbit.getRightAscensionOfAscendingNode();
  192.             stateVector[5] = circularOrbit.getAlpha(type);

  194.             if (stateVectorDot != null) {
  195.                 if (orbit.hasDerivatives()) {
  196.                     stateVectorDot[0] = circularOrbit.getADot();
  197.                     stateVectorDot[1] = circularOrbit.getCircularExDot();
  198.                     stateVectorDot[2] = circularOrbit.getCircularEyDot();
  199.                     stateVectorDot[3] = circularOrbit.getIDot();
  200.                     stateVectorDot[4] = circularOrbit.getRightAscensionOfAscendingNodeDot();
  201.                     stateVectorDot[5] = circularOrbit.getAlphaDot(type);
  202.                 } else {
  203.                     Arrays.fill(stateVectorDot, 0, 6, Double.NaN);
  204.                 }
  205.             }

  207.         }

  209.         /** {@inheritDoc} */
  210.         @Override
  211.         public CircularOrbit mapArrayToOrbit(final double[] stateVector, final double[] stateVectorDot, final PositionAngle type,
  212.                                              final AbsoluteDate date, final double mu, final Frame frame) {
  213.             if (stateVectorDot == null) {
  214.                 // we don't have orbit derivatives
  215.                 return new CircularOrbit(stateVector[0], stateVector[1], stateVector[2],
  216.                                          stateVector[3], stateVector[4], stateVector[5],
  217.                                          type, frame, date, mu);
  218.             } else {
  219.                 // we have orbit derivatives
  220.                 return new CircularOrbit(stateVector[0],    stateVector[1],    stateVector[2],
  221.                                          stateVector[3],    stateVector[4],    stateVector[5],
  222.                                          stateVectorDot[0], stateVectorDot[1], stateVectorDot[2],
  223.                                          stateVectorDot[3], stateVectorDot[4], stateVectorDot[5],
  224.                                          type, frame, date, mu);
  225.             }
  226.         }

  228.         /** {@inheritDoc} */
  229.         @Override
  230.         public <T extends RealFieldElement<T>> FieldCircularOrbit<T> convertType(final FieldOrbit<T> orbit) {
  231.             return (orbit.getType() == this) ? (FieldCircularOrbit<T>) orbit : new FieldCircularOrbit<>(orbit);
  232.         }

  234.         /** {@inheritDoc} */
  235.         @Override
  236.         public <T extends RealFieldElement<T>> void mapOrbitToArray(final FieldOrbit<T> orbit,
  237.                                                                     final PositionAngle type,
  238.                                                                     final T[] stateVector,
  239.                                                                     final T[] stateVectorDot) {

  241.             final FieldCircularOrbit<T> circularOrbit = (FieldCircularOrbit<T>) OrbitType.CIRCULAR.convertType(orbit);

  243.             stateVector[0] = circularOrbit.getA();
  244.             stateVector[1] = circularOrbit.getCircularEx();
  245.             stateVector[2] = circularOrbit.getCircularEy();
  246.             stateVector[3] = circularOrbit.getI();
  247.             stateVector[4] = circularOrbit.getRightAscensionOfAscendingNode();
  248.             stateVector[5] = circularOrbit.getAlpha(type);

  250.             if (stateVectorDot != null) {
  251.                 if (orbit.hasDerivatives()) {
  252.                     stateVectorDot[0] = circularOrbit.getADot();
  253.                     stateVectorDot[1] = circularOrbit.getCircularExDot();
  254.                     stateVectorDot[2] = circularOrbit.getCircularEyDot();
  255.                     stateVectorDot[3] = circularOrbit.getIDot();
  256.                     stateVectorDot[4] = circularOrbit.getRightAscensionOfAscendingNodeDot();
  257.                     stateVectorDot[5] = circularOrbit.getAlphaDot(type);
  258.                 } else {
  259.                     Arrays.fill(stateVectorDot, 0, 6, orbit.getDate().getField().getZero().add(Double.NaN));
  260.                 }
  261.             }

  263.         }

  265.         /** {@inheritDoc} */
  266.         @Override
  267.         public <T extends RealFieldElement<T>> FieldCircularOrbit<T> mapArrayToOrbit(final T[] stateVector,
  268.                                                                                      final T[] stateVectorDot, final PositionAngle type,
  269.                                                                                      final FieldAbsoluteDate<T> date,
  270.                                                                                      final T mu, final Frame frame) {
  271.             if (stateVectorDot == null) {
  272.                 // we don't have orbit derivatives
  273.                 return new FieldCircularOrbit<>(stateVector[0], stateVector[1], stateVector[2],
  274.                                                 stateVector[3], stateVector[4], stateVector[5],
  275.                                                 type, frame, date, mu);
  276.             } else {
  277.                 // we have orbit derivatives
  278.                 return new FieldCircularOrbit<>(stateVector[0],    stateVector[1],    stateVector[2],
  279.                                                 stateVector[3],    stateVector[4],    stateVector[5],
  280.                                                 stateVectorDot[0], stateVectorDot[1], stateVectorDot[2],
  281.                                                 stateVectorDot[3], stateVectorDot[4], stateVectorDot[5],
  282.                                                 type, frame, date, mu);
  283.             }
  284.         }

  286.         /** {@inheritDoc} */
  287.         @Override
  288.         public ParameterDriversList getDrivers(final double dP, final Orbit orbit, final PositionAngle type) {
  289.             final ParameterDriversList drivers = new ParameterDriversList();
  290.             final double[] array = new double[6];
  291.             mapOrbitToArray(orbit, type, array, null);
  292.             final double[] scale = scale(dP, orbit);
  293.             final String name = type == PositionAngle.MEAN ?
  294.                                     MEAN_LAT_ARG :
  295.                                     type == PositionAngle.ECCENTRIC ? ECC_LAT_ARG : TRUE_LAT_ARG;
  296.             drivers.add(new ParameterDriver(A,    array[0], scale[0],  0.0, Double.POSITIVE_INFINITY));
  297.             drivers.add(new ParameterDriver(E_X,  array[1], scale[1], -1.0, 1.0));
  298.             drivers.add(new ParameterDriver(E_Y,  array[2], scale[2], -1.0, 1.0));
  299.             drivers.add(new ParameterDriver(INC,  array[3], scale[3],  0.0, FastMath.PI));
  300.             drivers.add(new ParameterDriver(RAAN, array[4], scale[4], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  301.             drivers.add(new ParameterDriver(name, array[5], scale[5], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  302.             return drivers;
  303.         }

  305.     },

  307.     /** Type for propagation in {@link EquinoctialOrbit equinoctial parameters}. */
  308.     EQUINOCTIAL {

  310.         /** {@inheritDoc} */
  311.         @Override
  312.         public EquinoctialOrbit convertType(final Orbit orbit) {
  313.             return (orbit.getType() == this) ? (EquinoctialOrbit) orbit : new EquinoctialOrbit(orbit);
  314.         }

  316.         /** {@inheritDoc} */
  317.         @Override
  318.        public void mapOrbitToArray(final Orbit orbit, final PositionAngle type,
  319.                                    final double[] stateVector, final double[] stateVectorDot) {

  321.             final EquinoctialOrbit equinoctialOrbit =
  322.                 (EquinoctialOrbit) OrbitType.EQUINOCTIAL.convertType(orbit);

  324.             stateVector[0] = equinoctialOrbit.getA();
  325.             stateVector[1] = equinoctialOrbit.getEquinoctialEx();
  326.             stateVector[2] = equinoctialOrbit.getEquinoctialEy();
  327.             stateVector[3] = equinoctialOrbit.getHx();
  328.             stateVector[4] = equinoctialOrbit.getHy();
  329.             stateVector[5] = equinoctialOrbit.getL(type);

  331.             if (stateVectorDot != null) {
  332.                 if (orbit.hasDerivatives()) {
  333.                     stateVectorDot[0] = equinoctialOrbit.getADot();
  334.                     stateVectorDot[1] = equinoctialOrbit.getEquinoctialExDot();
  335.                     stateVectorDot[2] = equinoctialOrbit.getEquinoctialEyDot();
  336.                     stateVectorDot[3] = equinoctialOrbit.getHxDot();
  337.                     stateVectorDot[4] = equinoctialOrbit.getHyDot();
  338.                     stateVectorDot[5] = equinoctialOrbit.getLDot(type);
  339.                 } else {
  340.                     Arrays.fill(stateVectorDot, 0, 6, Double.NaN);
  341.                 }
  342.             }

  344.         }

  346.         /** {@inheritDoc} */
  347.         @Override
  348.         public EquinoctialOrbit mapArrayToOrbit(final double[] stateVector, final double[] stateVectorDot, final PositionAngle type,
  349.                                                 final AbsoluteDate date, final double mu, final Frame frame) {
  350.             if (stateVectorDot == null) {
  351.                 // we don't have orbit derivatives
  352.                 return new EquinoctialOrbit(stateVector[0], stateVector[1], stateVector[2],
  353.                                             stateVector[3], stateVector[4], stateVector[5],
  354.                                             type, frame, date, mu);
  355.             } else {
  356.                 // we have orbit derivatives
  357.                 return new EquinoctialOrbit(stateVector[0],    stateVector[1],    stateVector[2],
  358.                                             stateVector[3],    stateVector[4],    stateVector[5],
  359.                                             stateVectorDot[0], stateVectorDot[1], stateVectorDot[2],
  360.                                             stateVectorDot[3], stateVectorDot[4], stateVectorDot[5],
  361.                                             type, frame, date, mu);
  362.             }
  363.         }

  365.         /** {@inheritDoc} */
  366.         @Override
  367.         public <T extends RealFieldElement<T>> FieldEquinoctialOrbit<T> convertType(final FieldOrbit<T> orbit) {
  368.             return (orbit.getType() == this) ? (FieldEquinoctialOrbit<T>) orbit : new FieldEquinoctialOrbit<>(orbit);
  369.         }

  371.         /** {@inheritDoc} */
  372.         @Override
  373.         public <T extends RealFieldElement<T>> void mapOrbitToArray(final FieldOrbit<T> orbit,
  374.                                                                     final PositionAngle type,
  375.                                                                     final T[] stateVector,
  376.                                                                     final T[] stateVectorDot) {

  378.             final FieldEquinoctialOrbit<T> equinoctialOrbit =
  379.                 (FieldEquinoctialOrbit<T>) OrbitType.EQUINOCTIAL.convertType(orbit);

  381.             stateVector[0] = equinoctialOrbit.getA();
  382.             stateVector[1] = equinoctialOrbit.getEquinoctialEx();
  383.             stateVector[2] = equinoctialOrbit.getEquinoctialEy();
  384.             stateVector[3] = equinoctialOrbit.getHx();
  385.             stateVector[4] = equinoctialOrbit.getHy();
  386.             stateVector[5] = equinoctialOrbit.getL(type);

  388.             if (stateVectorDot != null) {
  389.                 if (orbit.hasDerivatives()) {
  390.                     stateVectorDot[0] = equinoctialOrbit.getADot();
  391.                     stateVectorDot[1] = equinoctialOrbit.getEquinoctialExDot();
  392.                     stateVectorDot[2] = equinoctialOrbit.getEquinoctialEyDot();
  393.                     stateVectorDot[3] = equinoctialOrbit.getHxDot();
  394.                     stateVectorDot[4] = equinoctialOrbit.getHyDot();
  395.                     stateVectorDot[5] = equinoctialOrbit.getLDot(type);
  396.                 } else {
  397.                     Arrays.fill(stateVectorDot, 0, 6, orbit.getDate().getField().getZero().add(Double.NaN));
  398.                 }
  399.             }

  401.         }

  403.         /** {@inheritDoc} */
  404.         @Override
  405.         public <T extends RealFieldElement<T>> FieldEquinoctialOrbit<T> mapArrayToOrbit(final T[] stateVector,
  406.                                                                                         final T[] stateVectorDot,
  407.                                                                                         final PositionAngle type,
  408.                                                                                         final FieldAbsoluteDate<T> date,
  409.                                                                                         final T mu, final Frame frame) {
  410.             if (stateVectorDot == null) {
  411.                 // we don't have orbit derivatives
  412.                 return new FieldEquinoctialOrbit<>(stateVector[0], stateVector[1], stateVector[2],
  413.                                                    stateVector[3], stateVector[4], stateVector[5],
  414.                                                    type, frame, date, mu);
  415.             } else {
  416.                 // we have orbit derivatives
  417.                 return new FieldEquinoctialOrbit<>(stateVector[0],    stateVector[1],    stateVector[2],
  418.                                                    stateVector[3],    stateVector[4],    stateVector[5],
  419.                                                    stateVectorDot[0], stateVectorDot[1], stateVectorDot[2],
  420.                                                    stateVectorDot[3], stateVectorDot[4], stateVectorDot[5],
  421.                                                    type, frame, date, mu);
  422.             }
  423.         }

  425.         /** {@inheritDoc} */
  426.         @Override
  427.         public ParameterDriversList getDrivers(final double dP, final Orbit orbit, final PositionAngle type) {
  428.             final ParameterDriversList drivers = new ParameterDriversList();
  429.             final double[] array = new double[6];
  430.             mapOrbitToArray(orbit, type, array, null);
  431.             final double[] scale = scale(dP, orbit);
  432.             final String name = type == PositionAngle.MEAN ?
  433.                                     MEAN_LON_ARG :
  434.                                     type == PositionAngle.ECCENTRIC ? ECC_LON_ARG : TRUE_LON_ARG;
  435.             drivers.add(new ParameterDriver(A,    array[0], scale[0],  0.0, Double.POSITIVE_INFINITY));
  436.             drivers.add(new ParameterDriver(E_X,  array[1], scale[1], -1.0, 1.0));
  437.             drivers.add(new ParameterDriver(E_Y,  array[2], scale[2], -1.0, 1.0));
  438.             drivers.add(new ParameterDriver(H_X,  array[3], scale[3], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  439.             drivers.add(new ParameterDriver(H_Y,  array[4], scale[4], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  440.             drivers.add(new ParameterDriver(name, array[5], scale[5], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  441.             return drivers;
  442.         }


  445.     },

  447.     /** Type for propagation in {@link KeplerianOrbit Keplerian parameters}. */
  448.     KEPLERIAN {

  450.         /** {@inheritDoc} */
  451.         @Override
  452.         public KeplerianOrbit convertType(final Orbit orbit) {
  453.             return (orbit.getType() == this) ? (KeplerianOrbit) orbit : new KeplerianOrbit(orbit);
  454.         }

  456.         /** {@inheritDoc} */
  457.         @Override
  458.         public void mapOrbitToArray(final Orbit orbit, final PositionAngle type,
  459.                                     final double[] stateVector, final double[] stateVectorDot) {

  461.             final KeplerianOrbit keplerianOrbit =
  462.                 (KeplerianOrbit) OrbitType.KEPLERIAN.convertType(orbit);

  464.             stateVector[0] = keplerianOrbit.getA();
  465.             stateVector[1] = keplerianOrbit.getE();
  466.             stateVector[2] = keplerianOrbit.getI();
  467.             stateVector[3] = keplerianOrbit.getPerigeeArgument();
  468.             stateVector[4] = keplerianOrbit.getRightAscensionOfAscendingNode();
  469.             stateVector[5] = keplerianOrbit.getAnomaly(type);

  471.             if (stateVectorDot != null) {
  472.                 if (orbit.hasDerivatives()) {
  473.                     stateVectorDot[0] = keplerianOrbit.getADot();
  474.                     stateVectorDot[1] = keplerianOrbit.getEDot();
  475.                     stateVectorDot[2] = keplerianOrbit.getIDot();
  476.                     stateVectorDot[3] = keplerianOrbit.getPerigeeArgumentDot();
  477.                     stateVectorDot[4] = keplerianOrbit.getRightAscensionOfAscendingNodeDot();
  478.                     stateVectorDot[5] = keplerianOrbit.getAnomalyDot(type);
  479.                 } else {
  480.                     Arrays.fill(stateVectorDot, 0, 6, Double.NaN);
  481.                 }
  482.             }

  484.         }

  486.         /** {@inheritDoc} */
  487.         @Override
  488.         public KeplerianOrbit mapArrayToOrbit(final double[] stateVector, final double[] stateVectorDot, final PositionAngle type,
  489.                                               final AbsoluteDate date, final double mu, final Frame frame) {
  490.             if (stateVectorDot == null) {
  491.                 // we don't have orbit derivatives
  492.                 return new KeplerianOrbit(stateVector[0], stateVector[1], stateVector[2],
  493.                                           stateVector[3], stateVector[4], stateVector[5],
  494.                                           type, frame, date, mu);
  495.             } else {
  496.                 // we have orbit derivatives
  497.                 return new KeplerianOrbit(stateVector[0],    stateVector[1],    stateVector[2],
  498.                                           stateVector[3],    stateVector[4],    stateVector[5],
  499.                                           stateVectorDot[0], stateVectorDot[1], stateVectorDot[2],
  500.                                           stateVectorDot[3], stateVectorDot[4], stateVectorDot[5],
  501.                                           type, frame, date, mu);
  502.             }
  503.         }

  505.         /** {@inheritDoc} */
  506.         @Override
  507.         public <T extends RealFieldElement<T>> FieldKeplerianOrbit<T> convertType(final FieldOrbit<T> orbit) {
  508.             return (orbit.getType() == this) ? (FieldKeplerianOrbit<T>) orbit : new FieldKeplerianOrbit<>(orbit);
  509.         }

  511.         /** {@inheritDoc} */
  512.         @Override
  513.         public <T extends RealFieldElement<T>> void mapOrbitToArray(final FieldOrbit<T> orbit,
  514.                                                                     final PositionAngle type,
  515.                                                                     final T[] stateVector,
  516.                                                                     final T[] stateVectorDot) {
  517.             final FieldKeplerianOrbit<T> keplerianOrbit =
  518.                             (FieldKeplerianOrbit<T>) OrbitType.KEPLERIAN.convertType(orbit);

  520.             stateVector[0] = keplerianOrbit.getA();
  521.             stateVector[1] = keplerianOrbit.getE();
  522.             stateVector[2] = keplerianOrbit.getI();
  523.             stateVector[3] = keplerianOrbit.getPerigeeArgument();
  524.             stateVector[4] = keplerianOrbit.getRightAscensionOfAscendingNode();
  525.             stateVector[5] = keplerianOrbit.getAnomaly(type);

  527.             if (stateVectorDot != null) {
  528.                 if (orbit.hasDerivatives()) {
  529.                     stateVectorDot[0] = keplerianOrbit.getADot();
  530.                     stateVectorDot[1] = keplerianOrbit.getEDot();
  531.                     stateVectorDot[2] = keplerianOrbit.getIDot();
  532.                     stateVectorDot[3] = keplerianOrbit.getPerigeeArgumentDot();
  533.                     stateVectorDot[4] = keplerianOrbit.getRightAscensionOfAscendingNodeDot();
  534.                     stateVectorDot[5] = keplerianOrbit.getAnomalyDot(type);
  535.                 } else {
  536.                     Arrays.fill(stateVectorDot, 0, 6, orbit.getDate().getField().getZero().add(Double.NaN));
  537.                 }
  538.             }

  540.         }

  542.         /** {@inheritDoc} */
  543.         @Override
  544.         public <T extends RealFieldElement<T>> FieldKeplerianOrbit<T> mapArrayToOrbit(final T[] stateVector,
  545.                                                                                       final T[] stateVectorDot,
  546.                                                                                       final PositionAngle type,
  547.                                                                                       final FieldAbsoluteDate<T> date,
  548.                                                                                       final T mu, final Frame frame) {
  549.             if (stateVectorDot == null) {
  550.                 // we don't have orbit derivatives
  551.                 return new FieldKeplerianOrbit<>(stateVector[0], stateVector[1], stateVector[2],
  552.                                                  stateVector[3], stateVector[4], stateVector[5],
  553.                                                  type, frame, date, mu);
  554.             } else {
  555.                 // we have orbit derivatives
  556.                 return new FieldKeplerianOrbit<>(stateVector[0],    stateVector[1],    stateVector[2],
  557.                                                  stateVector[3],    stateVector[4],    stateVector[5],
  558.                                                  stateVectorDot[0], stateVectorDot[1], stateVectorDot[2],
  559.                                                  stateVectorDot[3], stateVectorDot[4], stateVectorDot[5],
  560.                                                  type, frame, date, mu);
  561.             }
  562.         }

  564.         /** {@inheritDoc} */
  565.         @Override
  566.         public ParameterDriversList getDrivers(final double dP, final Orbit orbit, final PositionAngle type) {
  567.             final ParameterDriversList drivers = new ParameterDriversList();
  568.             final double[] array = new double[6];
  569.             mapOrbitToArray(orbit, type, array, null);
  570.             final double[] scale = scale(dP, orbit);
  571.             final String name = type == PositionAngle.MEAN ?
  572.                                     MEAN_ANOM :
  573.                                     type == PositionAngle.ECCENTRIC ? ECC_ANOM : TRUE_ANOM;
  574.             drivers.add(new ParameterDriver(A,    array[0], scale[0],  0.0, Double.POSITIVE_INFINITY));
  575.             drivers.add(new ParameterDriver(ECC,  array[1], scale[1],  0.0, 1.0));
  576.             drivers.add(new ParameterDriver(INC,  array[2], scale[2],  0.0, FastMath.PI));
  577.             drivers.add(new ParameterDriver(PA,   array[3], scale[3], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  578.             drivers.add(new ParameterDriver(RAAN, array[4], scale[4], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  579.             drivers.add(new ParameterDriver(name, array[5], scale[5], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  580.             return drivers;
  581.         }

  583.     };

  585.     /** Name for position along X. */
  586.     public static final String POS_X = "Px";

  588.     /** Name for position along Y. */
  589.     public static final String POS_Y = "Py";

  591.     /** Name for position along Z. */
  592.     public static final String POS_Z = "Pz";

  594.     /** Name for velocity along X. */
  595.     public static final String VEL_X = "Vx";

  597.     /** Name for velocity along Y. */
  598.     public static final String VEL_Y = "Vy";

  600.     /** Name for velocity along Z. */
  601.     public static final String VEL_Z = "Vz";

  603.     /** Name for semi major axis. */
  604.     public static final String A     = "a";

  606.     /** Name for eccentricity. */
  607.     public static final String ECC   = "e";

  609.     /** Name for eccentricity vector first component. */
  610.     public static final String E_X   = "ex";

  612.     /** Name for eccentricity vector second component. */
  613.     public static final String E_Y   = "ey";

  615.     /** Name for inclination. */
  616.     public static final String INC   = "i";

  618.     /** Name for inclination vector first component. */
  619.     public static final String H_X   = "hx";

  621.     /** Name for inclination vector second component . */
  622.     public static final String H_Y   = "hy";

  624.     /** Name for perigee argument. */
  625.     public static final String PA    = "ω";

  627.     /** Name for right ascension of ascending node. */
  628.     public static final String RAAN    = "Ω";

  630.     /** Name for mean anomaly. */
  631.     public static final String MEAN_ANOM = "M";

  633.     /** Name for eccentric anomaly. */
  634.     public static final String ECC_ANOM  = "E";

  636.     /** Name for mean anomaly. */
  637.     public static final String TRUE_ANOM = "v";

  639.     /** Name for mean argument of latitude. */
  640.     public static final String MEAN_LAT_ARG = "αM";

  642.     /** Name for eccentric argument of latitude. */
  643.     public static final String ECC_LAT_ARG  = "αE";

  645.     /** Name for mean argument of latitude. */
  646.     public static final String TRUE_LAT_ARG = "αv";

  648.     /** Name for mean argument of longitude. */
  649.     public static final String MEAN_LON_ARG = "λM";

  651.     /** Name for eccentric argument of longitude. */
  652.     public static final String ECC_LON_ARG  = "λE";

  654.     /** Name for mean argument of longitude. */
  655.     public static final String TRUE_LON_ARG = "λv";

  657.     /** Convert an orbit to the instance type.
  658.      * <p>
  659.      * The returned orbit is the specified instance itself if its type already matches,
  660.      * otherwise, a new orbit of the proper type created
  661.      * </p>
  662.      * @param orbit orbit to convert
  663.      * @return converted orbit with type guaranteed to match (so it can be cast safely)
  664.      */
  665.     public abstract Orbit convertType(Orbit orbit);

  667.     /** Convert orbit to state array.
  668.      * <p>
  669.      * Note that all implementations of this method <em>must</em> be consistent with the
  670.      * implementation of the {@link org.orekit.orbits.Orbit#getJacobianWrtCartesian(
  671.      * org.orekit.orbits.PositionAngle, double[][]) Orbit.getJacobianWrtCartesian}
  672.      * method for the corresponding orbit type in terms of parameters order and meaning.
  673.      * </p>
  674.      * @param orbit orbit to map
  675.      * @param type type of the angle
  676.      * @param stateVector flat array into which the state vector should be mapped
  677.      * (it can have more than 6 elements, extra elements are untouched)
  678.      * @param stateVectorDot flat array into which the state vector derivative should be mapped
  679.      * (it can be null if derivatives are not desired, and it can have more than 6 elements, extra elements are untouched)
  680.      */
  681.     public abstract void mapOrbitToArray(Orbit orbit, PositionAngle type, double[] stateVector, double[] stateVectorDot);

  683.      /** Convert state array to orbital parameters.
  684.      * <p>
  685.      * Note that all implementations of this method <em>must</em> be consistent with the
  686.      * implementation of the {@link org.orekit.orbits.Orbit#getJacobianWrtCartesian(
  687.      * org.orekit.orbits.PositionAngle, double[][]) Orbit.getJacobianWrtCartesian}
  688.      * method for the corresponding orbit type in terms of parameters order and meaning.
  689.      * </p>
  690.      * @param array state as a flat array
  691.      * (it can have more than 6 elements, extra elements are ignored)
  692.      * @param arrayDot state derivative as a flat array
  693.      * (it can be null, in which case Keplerian motion is assumed,
  694.      * and it can have more than 6 elements, extra elements are ignored)
  695.      * @param type type of the angle
  696.      * @param date integration date
  697.      * @param mu central attraction coefficient used for propagation (m³/s²)
  698.      * @param frame frame in which integration is performed
  699.      * @return orbit corresponding to the flat array as a space dynamics object
  700.      */
  701.     public abstract Orbit mapArrayToOrbit(double[] array, double arrayDot[], PositionAngle type,
  702.                                           AbsoluteDate date, double mu, Frame frame);

  704.     /** Convert an orbit to the instance type.
  705.      * <p>
  706.      * The returned orbit is the specified instance itself if its type already matches,
  707.      * otherwise, a new orbit of the proper type created
  708.      * </p>
  709.      * @param <T> RealFieldElement used
  710.      * @param orbit orbit to convert
  711.      * @return converted orbit with type guaranteed to match (so it can be cast safely)
  712.      */
  713.     public abstract <T extends RealFieldElement<T>> FieldOrbit<T> convertType(FieldOrbit<T> orbit);

  715.     /** Convert orbit to state array.
  716.      * <p>
  717.      * Note that all implementations of this method <em>must</em> be consistent with the
  718.      * implementation of the {@link org.orekit.orbits.Orbit#getJacobianWrtCartesian(
  719.      * org.orekit.orbits.PositionAngle, double[][]) Orbit.getJacobianWrtCartesian}
  720.      * method for the corresponding orbit type in terms of parameters order and meaning.
  721.      * </p>
  722.      * @param <T> RealFieldElement used
  723.      * @param orbit orbit to map
  724.      * @param type type of the angle
  725.      * @param stateVector flat array into which the state vector should be mapped
  726.      * (it can have more than 6 elements, extra elements are untouched)
  727.      * @param stateVectorDot flat array into which the state vector derivative should be mapped
  728.      * (it can be null if derivatives are not desired, and it can have more than 6 elements, extra elements are untouched)
  729.      */
  730.     public abstract <T extends RealFieldElement<T>>void mapOrbitToArray(FieldOrbit<T> orbit, PositionAngle type,
  731.                                                                         T[] stateVector, T[] stateVectorDot);


  734.     /** Convert state array to orbital parameters.
  735.      * <p>
  736.      * Note that all implementations of this method <em>must</em> be consistent with the
  737.      * implementation of the {@link org.orekit.orbits.Orbit#getJacobianWrtCartesian(
  738.      * org.orekit.orbits.PositionAngle, double[][]) Orbit.getJacobianWrtCartesian}
  739.      * method for the corresponding orbit type in terms of parameters order and meaning.
  740.      * </p>
  741.      * @param <T> RealFieldElement used
  742.      * @param array state as a flat array
  743.      * (it can have more than 6 elements, extra elements are ignored)
  744.      * @param arrayDot state derivative as a flat array
  745.      * (it can be null, in which case Keplerian motion is assumed,
  746.      * @param type type of the angle
  747.      * @param date integration date
  748.      * @param mu central attraction coefficient used for propagation (m³/s²)
  749.      * @param frame frame in which integration is performed
  750.      * @return orbit corresponding to the flat array as a space dynamics object
  751.      */
  752.     public abstract <T extends RealFieldElement<T>> FieldOrbit<T> mapArrayToOrbit(T[] array,
  753.                                                                                   T[] arrayDot,
  754.                                                                                   PositionAngle type,
  755.                                                                                   FieldAbsoluteDate<T> date,
  756.                                                                                   T mu, Frame frame);

  758.     /** Get parameters drivers initialized from a reference orbit.
  759.      * @param dP user specified position error
  760.      * @param orbit reference orbit
  761.      * @param type type of the angle
  762.      * @return parameters drivers initialized from reference orbit
  763.      */
  764.     public abstract ParameterDriversList getDrivers(double dP, Orbit orbit,
  765.                                                     PositionAngle type);

  767.     /** Compute scaling factor for parameters drivers.
  768.      * <p>
  769.      * The scales are estimated from partial derivatives properties of orbits,
  770.      * starting from a scalar position error specified by the user.
  771.      * Considering the energy conservation equation V = sqrt(mu (2/r - 1/a)),
  772.      * we get at constant energy (i.e. on a Keplerian trajectory):
  773.      * <pre>
  774.      * V² r |dV| = mu |dr|
  775.      * </pre>
  776.      * <p> So we deduce a scalar velocity error consistent with the position error.
  777.      * From here, we apply orbits Jacobians matrices to get consistent scales
  778.      * on orbital parameters.
  779.      *
  780.      * @param dP user specified position error
  781.      * @param orbit reference orbit
  782.      * @return scaling factor array
  783.      */
  784.     protected double[] scale(final double dP, final Orbit orbit) {

  786.         // estimate the scalar velocity error
  787.         final PVCoordinates pv = orbit.getPVCoordinates();
  788.         final double r2 = pv.getPosition().getNormSq();
  789.         final double v  = pv.getVelocity().getNorm();
  790.         final double dV = orbit.getMu() * dP / (v * r2);

  792.         final double[] scale = new double[6];

  794.         // convert the orbit to the desired type
  795.         final double[][] jacobian = new double[6][6];
  796.         final Orbit converted = convertType(orbit);
  797.         converted.getJacobianWrtCartesian(PositionAngle.TRUE, jacobian);

  799.         for (int i = 0; i < 6; ++i) {
  800.             final double[] row = jacobian[i];
  801.             scale[i] = FastMath.abs(row[0]) * dP +
  802.                        FastMath.abs(row[1]) * dP +
  803.                        FastMath.abs(row[2]) * dP +
  804.                        FastMath.abs(row[3]) * dV +
  805.                        FastMath.abs(row[4]) * dV +
  806.                        FastMath.abs(row[5]) * dV;
  807.             if (Double.isNaN(scale[i])) {
  808.                 throw new OrekitException(OrekitMessages.SINGULAR_JACOBIAN_FOR_ORBIT_TYPE, this);
  809.             }
  810.         }

  812.         return scale;

  814.     }

  816. }
Created with JaCoCo 0.8.6.202009150832