NadirPointing.java

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

  19. import java.util.ArrayList;
  20. import java.util.List;

  22. import org.hipparchus.CalculusFieldElement;
  23. import org.hipparchus.Field;
  24. import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
  25. import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2Field;
  26. import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
  27. import org.hipparchus.analysis.differentiation.UnivariateDerivative2Field;
  28. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  29. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  30. import org.orekit.bodies.BodyShape;
  31. import org.orekit.bodies.FieldGeodeticPoint;
  32. import org.orekit.bodies.GeodeticPoint;
  33. import org.orekit.frames.FieldStaticTransform;
  34. import org.orekit.frames.FieldTransform;
  35. import org.orekit.frames.Frame;
  36. import org.orekit.frames.StaticTransform;
  37. import org.orekit.frames.Transform;
  38. import org.orekit.time.AbsoluteDate;
  39. import org.orekit.time.FieldAbsoluteDate;
  40. import org.orekit.time.FieldTimeInterpolator;
  41. import org.orekit.time.TimeInterpolator;
  42. import org.orekit.utils.CartesianDerivativesFilter;
  43. import org.orekit.utils.FieldPVCoordinatesProvider;
  44. import org.orekit.utils.FieldPVCoordinates;
  45. import org.orekit.utils.PVCoordinatesProvider;
  46. import org.orekit.utils.PVCoordinates;
  47. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  48. import org.orekit.utils.TimeStampedFieldPVCoordinatesHermiteInterpolator;
  49. import org.orekit.utils.TimeStampedPVCoordinates;
  50. import org.orekit.utils.TimeStampedPVCoordinatesHermiteInterpolator;

  52. /**
  53.  * This class handles nadir pointing attitude provider.

  55.  * <p>
  56.  * This class represents the attitude provider where the satellite z axis is
  57.  * pointing to the vertical of the ground point under satellite.</p>
  58.  * <p>
  59.  * The object <code>NadirPointing</code> is guaranteed to be immutable.
  60.  * </p>
  61.  * @see     GroundPointing
  62.  * @author V&eacute;ronique Pommier-Maurussane
  63.  */
  64. public class NadirPointing extends GroundPointing {

  66.     /** Body shape.  */
  67.     private final BodyShape shape;

  69.     /** Creates new instance.
  70.      * @param inertialFrame frame in which orbital velocities are computed
  71.      * @param shape Body shape
  72.      * @since 7.1
  73.      */
  74.     public NadirPointing(final Frame inertialFrame, final BodyShape shape) {
  75.         // Call constructor of superclass
  76.         super(inertialFrame, shape.getBodyFrame());
  77.         this.shape = shape;
  78.     }

  80.     /** {@inheritDoc} */
  81.     @Override
  82.     public TimeStampedPVCoordinates getTargetPV(final PVCoordinatesProvider pvProv,
  83.                                                 final AbsoluteDate date, final Frame frame) {

  85.         final TimeStampedPVCoordinates pvCoordinatesInRef = pvProv.getPVCoordinates(date, frame);
  86.         if (pvCoordinatesInRef.getAcceleration().equals(Vector3D.ZERO)) {
  87.             // let us assume that there is no proper acceleration available, so need to use interpolation for derivatives
  88.             return getTargetPVViaInterpolation(pvProv, date, frame);

  90.         } else {  // use automatic differentiation
  91.             // build time dependent transform
  92.             final UnivariateDerivative2Field ud2Field = UnivariateDerivative2Field.getInstance();
  93.             final UnivariateDerivative2 dt = new UnivariateDerivative2(0., 1., 0.);
  94.             final FieldAbsoluteDate<UnivariateDerivative2> ud2Date = new FieldAbsoluteDate<>(ud2Field, date).shiftedBy(dt);
  95.             final FieldStaticTransform<UnivariateDerivative2> refToBody = frame.getStaticTransformTo(shape.getBodyFrame(), ud2Date);

  97.             final FieldVector3D<UnivariateDerivative2> positionInRefFrame = pvCoordinatesInRef.toUnivariateDerivative2Vector();
  98.             final FieldVector3D<UnivariateDerivative2> positionInBodyFrame = refToBody.transformPosition(positionInRefFrame);

  100.             // satellite position in geodetic coordinates
  101.             final FieldGeodeticPoint<UnivariateDerivative2> gpSat = shape.transform(positionInBodyFrame, getBodyFrame(), ud2Date);

  103.             // nadir position in geodetic coordinates
  104.             final FieldGeodeticPoint<UnivariateDerivative2> gpNadir = new FieldGeodeticPoint<>(gpSat.getLatitude(),
  105.                 gpSat.getLongitude(), ud2Field.getZero());

  107.             // nadir point position in body frame
  108.             final FieldVector3D<UnivariateDerivative2> positionNadirInBodyFrame = shape.transform(gpNadir);

  110.             // nadir point position in reference frame
  111.             final FieldStaticTransform<UnivariateDerivative2> bodyToRef = refToBody.getInverse();
  112.             final FieldVector3D<UnivariateDerivative2> positionNadirInRefFrame = bodyToRef.transformPosition(positionNadirInBodyFrame);

  114.             // put derivatives into proper object
  115.             final Vector3D velocity = new Vector3D(positionNadirInRefFrame.getX().getFirstDerivative(),
  116.                     positionNadirInRefFrame.getY().getFirstDerivative(), positionNadirInRefFrame.getZ().getFirstDerivative());
  117.             final Vector3D acceleration = new Vector3D(positionNadirInRefFrame.getX().getSecondDerivative(),
  118.                 positionNadirInRefFrame.getY().getSecondDerivative(), positionNadirInRefFrame.getZ().getSecondDerivative());
  119.             return new TimeStampedPVCoordinates(date, positionNadirInRefFrame.toVector3D(), velocity, acceleration);
  120.         }
  121.     }

  123.     /**
  124.      * Compute target position-velocity-acceleration vector via interpolation.
  125.      * @param pvProv PV provider
  126.      * @param date date
  127.      * @param frame frame
  128.      * @return target position-velocity-acceleration
  129.      */
  130.     public TimeStampedPVCoordinates getTargetPVViaInterpolation(final PVCoordinatesProvider pvProv,
  131.                                                                 final AbsoluteDate date, final Frame frame) {

  133.         // transform from specified reference frame to body frame
  134.         final Transform refToBody = frame.getTransformTo(shape.getBodyFrame(), date);

  136.         // sample intersection points in current date neighborhood
  137.         final double h  = 0.01;
  138.         final List<TimeStampedPVCoordinates> sample = new ArrayList<>();
  139.         sample.add(nadirRef(pvProv.getPVCoordinates(date.shiftedBy(-2 * h), frame), refToBody.staticShiftedBy(-2 * h)));
  140.         sample.add(nadirRef(pvProv.getPVCoordinates(date.shiftedBy(-h),     frame), refToBody.staticShiftedBy(-h)));
  141.         sample.add(nadirRef(pvProv.getPVCoordinates(date,                   frame), refToBody));
  142.         sample.add(nadirRef(pvProv.getPVCoordinates(date.shiftedBy(+h),     frame), refToBody.staticShiftedBy(+h)));
  143.         sample.add(nadirRef(pvProv.getPVCoordinates(date.shiftedBy(+2 * h), frame), refToBody.staticShiftedBy(+2 * h)));

  145.         // create interpolator
  146.         final TimeInterpolator<TimeStampedPVCoordinates> interpolator =
  147.                 new TimeStampedPVCoordinatesHermiteInterpolator(sample.size(), CartesianDerivativesFilter.USE_P);

  149.         // use interpolation to compute properly the time-derivatives
  150.         return interpolator.interpolate(date, sample);

  152.     }

  154.     /** {@inheritDoc} */
  155.     @Override
  156.     protected Vector3D getTargetPosition(final PVCoordinatesProvider pvProv, final AbsoluteDate date, final Frame frame) {

  158.         // transform from specified reference frame to body frame
  159.         final Vector3D position = pvProv.getPosition(date, frame);
  160.         final PVCoordinates pVWithoutDerivatives = new PVCoordinates(position);
  161.         final StaticTransform refToBody = frame.getStaticTransformTo(shape.getBodyFrame(), date);

  163.         return nadirRef(new TimeStampedPVCoordinates(date, pVWithoutDerivatives), refToBody).getPosition();
  164.     }

  166.     /** {@inheritDoc} */
  167.     @Override
  168.     public <T extends CalculusFieldElement<T>> TimeStampedFieldPVCoordinates<T> getTargetPV(final FieldPVCoordinatesProvider<T> pvProv,
  169.                                                                                             final FieldAbsoluteDate<T> date,
  170.                                                                                             final Frame frame) {

  172.         final TimeStampedFieldPVCoordinates<T> pvCoordinatesInRef = pvProv.getPVCoordinates(date, frame);
  173.         final Field<T> field = date.getField();
  174.         if (pvCoordinatesInRef.getAcceleration().equals(FieldVector3D.getZero(field))) {
  175.             // let us assume that there is no proper acceleration available, so need to use interpolation for derivatives
  176.             return getTargetPVViaInterpolation(pvProv, date, frame);

  178.         } else {  // use automatic differentiation
  179.             // build time dependent transform
  180.             final FieldUnivariateDerivative2Field<T> ud2Field = FieldUnivariateDerivative2Field.getUnivariateDerivative2Field(field);
  181.             final FieldAbsoluteDate<FieldUnivariateDerivative2<T>> ud2Date = date.toFUD2Field();
  182.             final FieldStaticTransform<FieldUnivariateDerivative2<T>> refToBody = frame.getStaticTransformTo(shape.getBodyFrame(), ud2Date);

  184.             final FieldVector3D<FieldUnivariateDerivative2<T>> positionInRefFrame = pvCoordinatesInRef.toUnivariateDerivative2Vector();
  185.             final FieldVector3D<FieldUnivariateDerivative2<T>> positionInBodyFrame = refToBody.transformPosition(positionInRefFrame);

  187.             // satellite position in geodetic coordinates
  188.             final FieldGeodeticPoint<FieldUnivariateDerivative2<T>> gpSat = shape.transform(positionInBodyFrame, getBodyFrame(), ud2Date);

  190.             // nadir position in geodetic coordinates
  191.             final FieldGeodeticPoint<FieldUnivariateDerivative2<T>> gpNadir = new FieldGeodeticPoint<>(gpSat.getLatitude(),
  192.                     gpSat.getLongitude(), ud2Field.getZero());

  194.             // nadir point position in body frame
  195.             final FieldVector3D<FieldUnivariateDerivative2<T>> positionNadirInBodyFrame = shape.transform(gpNadir);

  197.             // nadir point position in reference frame
  198.             final FieldStaticTransform<FieldUnivariateDerivative2<T>> bodyToRef = refToBody.getInverse();
  199.             final FieldVector3D<FieldUnivariateDerivative2<T>> positionNadirInRefFrame = bodyToRef.transformPosition(positionNadirInBodyFrame);

  201.             // put derivatives into proper object
  202.             final FieldVector3D<T> position = new FieldVector3D<>(positionNadirInRefFrame.getX().getValue(),
  203.                     positionNadirInRefFrame.getY().getValue(), positionNadirInRefFrame.getZ().getValue());
  204.             final FieldVector3D<T> velocity = new FieldVector3D<>(positionNadirInRefFrame.getX().getFirstDerivative(),
  205.                     positionNadirInRefFrame.getY().getFirstDerivative(), positionNadirInRefFrame.getZ().getFirstDerivative());
  206.             final FieldVector3D<T> acceleration = new FieldVector3D<>(positionNadirInRefFrame.getX().getSecondDerivative(),
  207.                     positionNadirInRefFrame.getY().getSecondDerivative(), positionNadirInRefFrame.getZ().getSecondDerivative());
  208.             return new TimeStampedFieldPVCoordinates<>(date, position, velocity, acceleration);
  209.         }

  211.     }

  213.     /**
  214.      * Compute target position-velocity-acceleration vector via interpolation (Field version).
  215.      * @param pvProv PV provider
  216.      * @param date date
  217.      * @param frame frame
  218.      * @param <T> field type
  219.      * @return target position-velocity-acceleration
  220.      */
  221.     public <T extends CalculusFieldElement<T>> TimeStampedFieldPVCoordinates<T> getTargetPVViaInterpolation(final FieldPVCoordinatesProvider<T> pvProv,
  222.                                                                                                             final FieldAbsoluteDate<T> date, final Frame frame) {

  224.         // zero
  225.         final T zero = date.getField().getZero();

  227.         // transform from specified reference frame to body frame
  228.         final FieldTransform<T> refToBody = frame.getTransformTo(shape.getBodyFrame(), date);

  230.         // sample intersection points in current date neighborhood
  231.         final double h  = 0.01;
  232.         final List<TimeStampedFieldPVCoordinates<T>> sample = new ArrayList<>();
  233.         sample.add(nadirRef(pvProv.getPVCoordinates(date.shiftedBy(-2 * h), frame), refToBody.staticShiftedBy(zero.newInstance(-2 * h))));
  234.         sample.add(nadirRef(pvProv.getPVCoordinates(date.shiftedBy(-h),     frame), refToBody.staticShiftedBy(zero.newInstance(-h))));
  235.         sample.add(nadirRef(pvProv.getPVCoordinates(date,                   frame), refToBody));
  236.         sample.add(nadirRef(pvProv.getPVCoordinates(date.shiftedBy(+h),     frame), refToBody.staticShiftedBy(zero.newInstance(+h))));
  237.         sample.add(nadirRef(pvProv.getPVCoordinates(date.shiftedBy(+2 * h), frame), refToBody.staticShiftedBy(zero.newInstance(+2 * h))));

  239.         // create interpolator
  240.         final FieldTimeInterpolator<TimeStampedFieldPVCoordinates<T>, T> interpolator =
  241.                 new TimeStampedFieldPVCoordinatesHermiteInterpolator<>(sample.size(), CartesianDerivativesFilter.USE_P);

  243.         // use interpolation to compute properly the time-derivatives
  244.         return interpolator.interpolate(date, sample);

  246.     }

  248.     /** {@inheritDoc} */
  249.     @Override
  250.     protected <T extends CalculusFieldElement<T>> FieldVector3D<T> getTargetPosition(final FieldPVCoordinatesProvider<T> pvProv,
  251.                                                                                      final FieldAbsoluteDate<T> date,
  252.                                                                                      final Frame frame) {

  254.         // transform from specified reference frame to body frame
  255.         final FieldVector3D<T> position = pvProv.getPosition(date, frame);
  256.         final FieldPVCoordinates<T> pVWithoutDerivatives = new FieldPVCoordinates<>(position, FieldVector3D.getZero(date.getField()));
  257.         final FieldStaticTransform<T> refToBody = frame.getStaticTransformTo(shape.getBodyFrame(), date);

  259.         return nadirRef(new TimeStampedFieldPVCoordinates<>(date, pVWithoutDerivatives), refToBody).getPosition();

  261.     }

  263.     /** Compute ground point in nadir direction, in reference frame.
  264.      * @param scRef spacecraft coordinates in reference frame
  265.      * @param refToBody transform from reference frame to body frame
  266.      * @return intersection point in body frame (only the position is set!)
  267.      */
  268.     private TimeStampedPVCoordinates nadirRef(final TimeStampedPVCoordinates scRef,
  269.                                               final StaticTransform refToBody) {

  271.         final Vector3D satInBodyFrame = refToBody.transformPosition(scRef.getPosition());

  273.         // satellite position in geodetic coordinates
  274.         final GeodeticPoint gpSat = shape.transform(satInBodyFrame, getBodyFrame(), scRef.getDate());

  276.         // nadir position in geodetic coordinates
  277.         final GeodeticPoint gpNadir = new GeodeticPoint(gpSat.getLatitude(), gpSat.getLongitude(), 0.0);

  279.         // nadir point position in body frame
  280.         final Vector3D pNadirBody = shape.transform(gpNadir);

  282.         // nadir point position in reference frame
  283.         final Vector3D pNadirRef = refToBody.getInverse().transformPosition(pNadirBody);

  285.         return new TimeStampedPVCoordinates(scRef.getDate(), pNadirRef, Vector3D.ZERO, Vector3D.ZERO);

  287.     }

  289.     /** Compute ground point in nadir direction, in reference frame.
  290.      * @param scRef spacecraft coordinates in reference frame
  291.      * @param refToBody transform from reference frame to body frame
  292.      * @param <T> type of the field elements
  293.      * @return intersection point in body frame (only the position is set!)
  294.      * @since 9.0
  295.      */
  296.     private <T extends CalculusFieldElement<T>> TimeStampedFieldPVCoordinates<T> nadirRef(final TimeStampedFieldPVCoordinates<T> scRef,
  297.                                                                                           final FieldStaticTransform<T> refToBody) {

  299.         final FieldVector3D<T> satInBodyFrame = refToBody.transformPosition(scRef.getPosition());

  301.         // satellite position in geodetic coordinates
  302.         final FieldGeodeticPoint<T> gpSat = shape.transform(satInBodyFrame, getBodyFrame(), scRef.getDate());

  304.         // nadir position in geodetic coordinates
  305.         final FieldGeodeticPoint<T> gpNadir = new FieldGeodeticPoint<>(gpSat.getLatitude(), gpSat.getLongitude(),
  306.                                                                        gpSat.getAltitude().getField().getZero());

  308.         // nadir point position in body frame
  309.         final FieldVector3D<T> pNadirBody = shape.transform(gpNadir);

  311.         // nadir point position in reference frame
  312.         final FieldVector3D<T> pNadirRef = refToBody.getInverse().transformPosition(pNadirBody);

  314.         final FieldVector3D<T> zero = FieldVector3D.getZero(gpSat.getAltitude().getField());
  315.         return new TimeStampedFieldPVCoordinates<>(scRef.getDate(), pNadirRef, zero, zero);

  317.     }

  319. }
Created with JaCoCo 0.8.12.202403310830