OrbitType.java

  1. /* Copyright 2002-2018 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.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 double 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.             throws OrekitException {
  157.             final ParameterDriversList drivers = new ParameterDriversList();
  158.             final double[] array = new double[6];
  159.             mapOrbitToArray(orbit, type, array, null);
  160.             final double[] scale = scale(dP, orbit);
  161.             drivers.add(new ParameterDriver(POS_X, array[0], scale[0], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  162.             drivers.add(new ParameterDriver(POS_Y, array[1], scale[1], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  163.             drivers.add(new ParameterDriver(POS_Z, array[2], scale[2], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  164.             drivers.add(new ParameterDriver(VEL_X, array[3], scale[3], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  165.             drivers.add(new ParameterDriver(VEL_Y, array[4], scale[4], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  166.             drivers.add(new ParameterDriver(VEL_Z, array[5], scale[5], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
  167.             return drivers;
  168.         }

  170.     },

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

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

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

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

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

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

  208.         }

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

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

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

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

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

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

  264.         }

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

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

  307.     },

  309.     /** Type for propagation in {@link EquinoctialOrbit equinoctial parameters}. */
  310.     EQUINOCTIAL {

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

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

  323.             final EquinoctialOrbit equinoctialOrbit =
  324.                 (EquinoctialOrbit) OrbitType.EQUINOCTIAL.convertType(orbit);

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

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

  346.         }

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

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

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

  380.             final FieldEquinoctialOrbit<T> equinoctialOrbit =
  381.                 (FieldEquinoctialOrbit<T>) OrbitType.EQUINOCTIAL.convertType(orbit);

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

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

  403.         }

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

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


  448.     },

  450.     /** Type for propagation in {@link KeplerianOrbit Keplerian parameters}. */
  451.     KEPLERIAN {

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

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

  464.             final KeplerianOrbit keplerianOrbit =
  465.                 (KeplerianOrbit) OrbitType.KEPLERIAN.convertType(orbit);

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

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

  487.         }

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

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

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

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

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

  543.         }

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

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

  587.     };

  589.     /** Name for position along X. */
  590.     private static final String POS_X = "Px";

  592.     /** Name for position along Y. */
  593.     private static final String POS_Y = "Py";

  595.     /** Name for position along Z. */
  596.     private static final String POS_Z = "Pz";

  598.     /** Name for velocity along X. */
  599.     private static final String VEL_X = "Vx";

  601.     /** Name for velocity along Y. */
  602.     private static final String VEL_Y = "Vy";

  604.     /** Name for velocity along Z. */
  605.     private static final String VEL_Z = "Vz";

  607.     /** Name for semi major axis. */
  608.     private static final String A     = "a";

  610.     /** Name for eccentricity. */
  611.     private static final String ECC   = "e";

  613.     /** Name for eccentricity vector first component. */
  614.     private static final String E_X   = "ex";

  616.     /** Name for eccentricity vector second component. */
  617.     private static final String E_Y   = "ey";

  619.     /** Name for inclination. */
  620.     private static final String INC   = "i";

  622.     /** Name for inclination vector first component. */
  623.     private static final String H_X   = "hx";

  625.     /** Name for inclination vector second component . */
  626.     private static final String H_Y   = "hy";

  628.     /** Name for perigee argument. */
  629.     private static final String PA    = "ω";

  631.     /** Name for right ascension of ascending node. */
  632.     private static final String RAAN    = "Ω";

  634.     /** Name for mean anomaly. */
  635.     private static final String MEAN_ANOM = "M";

  637.     /** Name for eccentric anomaly. */
  638.     private static final String ECC_ANOM  = "E";

  640.     /** Name for mean anomaly. */
  641.     private static final String TRUE_ANOM = "v";

  643.     /** Name for mean argument of latitude. */
  644.     private static final String MEAN_LAT_ARG = "αM";

  646.     /** Name for eccentric argument of latitude. */
  647.     private static final String ECC_LAT_ARG  = "αE";

  649.     /** Name for mean argument of latitude. */
  650.     private static final String TRUE_LAT_ARG = "αv";

  652.     /** Name for mean argument of longitude. */
  653.     private static final String MEAN_LON_ARG = "λM";

  655.     /** Name for eccentric argument of longitude. */
  656.     private static final String ECC_LON_ARG  = "λE";

  658.     /** Name for mean argument of longitude. */
  659.     private static final String TRUE_LON_ARG = "λv";

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

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

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

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

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


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

  762.     /** Get parameters drivers initialized from a reference orbit.
  763.      * @param dP user specified position error
  764.      * @param orbit reference orbit
  765.      * @param type type of the angle
  766.      * @return parameters drivers initialized from reference orbit
  767.      * @exception OrekitException if Jacobian is singular
  768.      */
  769.     public abstract ParameterDriversList getDrivers(double dP, Orbit orbit,
  770.                                                     PositionAngle type)
  771.         throws OrekitException;

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

  794.         // estimate the scalar velocity error
  795.         final PVCoordinates pv = orbit.getPVCoordinates();
  796.         final double r2 = pv.getPosition().getNormSq();
  797.         final double v  = pv.getVelocity().getNorm();
  798.         final double dV = orbit.getMu() * dP / (v * r2);

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

  802.         // convert the orbit to the desired type
  803.         final double[][] jacobian = new double[6][6];
  804.         final Orbit converted = convertType(orbit);
  805.         converted.getJacobianWrtCartesian(PositionAngle.TRUE, jacobian);

  807.         for (int i = 0; i < 6; ++i) {
  808.             final double[] row = jacobian[i];
  809.             scale[i] = FastMath.abs(row[0]) * dP +
  810.                        FastMath.abs(row[1]) * dP +
  811.                        FastMath.abs(row[2]) * dP +
  812.                        FastMath.abs(row[3]) * dV +
  813.                        FastMath.abs(row[4]) * dV +
  814.                        FastMath.abs(row[5]) * dV;
  815.             if (Double.isNaN(scale[i])) {
  816.                 throw new OrekitException(OrekitMessages.SINGULAR_JACOBIAN_FOR_ORBIT_TYPE, this);
  817.             }
  818.         }

  820.         return scale;

  822.     }

  824. }
Created with JaCoCo 0.7.9.201702052155