LofOffsetPointing.java

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

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

  20. import org.hipparchus.CalculusFieldElement;
  21. import org.hipparchus.geometry.euclidean.threed.FieldLine;
  22. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  23. import org.hipparchus.geometry.euclidean.threed.Line;
  24. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  25. import org.hipparchus.geometry.euclidean.threed.Rotation;
  26. import org.hipparchus.geometry.euclidean.threed.FieldRotation;
  27. import org.orekit.bodies.BodyShape;
  28. import org.orekit.bodies.FieldGeodeticPoint;
  29. import org.orekit.bodies.GeodeticPoint;
  30. import org.orekit.errors.OrekitException;
  31. import org.orekit.errors.OrekitMessages;
  32. import org.orekit.frames.FieldStaticTransform;
  33. import org.orekit.frames.FieldTransform;
  34. import org.orekit.frames.Frame;
  35. import org.orekit.frames.StaticTransform;
  36. import org.orekit.frames.Transform;
  37. import org.orekit.time.AbsoluteDate;
  38. import org.orekit.time.FieldAbsoluteDate;
  39. import org.orekit.time.FieldTimeInterpolator;
  40. import org.orekit.time.TimeInterpolator;
  41. import org.orekit.utils.CartesianDerivativesFilter;
  42. import org.orekit.utils.Constants;
  43. import org.orekit.utils.FieldPVCoordinatesProvider;
  44. import org.orekit.utils.PVCoordinatesProvider;
  45. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  46. import org.orekit.utils.TimeStampedFieldPVCoordinatesHermiteInterpolator;
  47. import org.orekit.utils.TimeStampedPVCoordinates;
  48. import org.orekit.utils.TimeStampedPVCoordinatesHermiteInterpolator;

  49. /**
  50.  * This class provides a default attitude provider.

  51.  * <p>
  52.  * The attitude pointing law is defined by an attitude provider and
  53.  * the satellite axis vector chosen for pointing.
  54.  * </p>
  55.  * @author V&eacute;ronique Pommier-Maurussane
  56.  */
  57. public class LofOffsetPointing extends GroundPointing {

  58.     /** Rotation from local orbital frame. */
  59.     private final AttitudeProvider attitudeLaw;

  60.     /** Body shape. */
  61.     private final BodyShape shape;

  62.     /** Chosen satellite axis for pointing, given in satellite frame. */
  63.     private final Vector3D satPointingVector;

  64.     /** Creates new instance.
  65.      * @param inertialFrame frame in which orbital velocities are computed
  66.      * @param shape Body shape
  67.      * @param attLaw Attitude law
  68.      * @param satPointingVector satellite vector defining the pointing direction
  69.      * @since 7.1
  70.      */
  71.     public LofOffsetPointing(final Frame inertialFrame, final BodyShape shape,
  72.                              final AttitudeProvider attLaw, final Vector3D satPointingVector) {
  73.         super(inertialFrame, shape.getBodyFrame());
  74.         this.shape = shape;
  75.         this.attitudeLaw = attLaw;
  76.         this.satPointingVector = satPointingVector;
  77.     }

  78.     /** {@inheritDoc} */
  79.     @Override
  80.     public Attitude getAttitude(final PVCoordinatesProvider pvProv,
  81.                                 final AbsoluteDate date, final Frame frame) {
  82.         return attitudeLaw.getAttitude(pvProv, date, frame);
  83.     }

  84.     /** {@inheritDoc} */
  85.     @Override
  86.     public <T extends CalculusFieldElement<T>> FieldAttitude<T> getAttitude(final FieldPVCoordinatesProvider<T> pvProv,
  87.                                                                             final FieldAbsoluteDate<T> date, final Frame frame) {
  88.         return attitudeLaw.getAttitude(pvProv, date, frame);
  89.     }

  90.     /** {@inheritDoc} */
  91.     @Override
  92.     public Rotation getAttitudeRotation(final PVCoordinatesProvider pvProv,
  93.                                         final AbsoluteDate date, final Frame frame) {
  94.         return attitudeLaw.getAttitudeRotation(pvProv, date, frame);
  95.     }

  96.     /** {@inheritDoc} */
  97.     @Override
  98.     public <T extends CalculusFieldElement<T>> FieldRotation<T> getAttitudeRotation(final FieldPVCoordinatesProvider<T> pvProv,
  99.                                                                                     final FieldAbsoluteDate<T> date, final Frame frame) {
  100.         return attitudeLaw.getAttitudeRotation(pvProv, date, frame);
  101.     }

  102.     /** {@inheritDoc} */
  103.     @Override
  104.     public TimeStampedPVCoordinates getTargetPV(final PVCoordinatesProvider pvProv,
  105.                                                 final AbsoluteDate date, final Frame frame) {

  106.         // sample intersection points in current date neighborhood
  107.         final double h  = 0.1;
  108.         final List<TimeStampedPVCoordinates> sample = new ArrayList<>();
  109.         Transform centralRefToBody = null;
  110.         for (int i = -1; i < 2; ++i) {

  111.             final AbsoluteDate shifted = date.shiftedBy(i * h);

  112.             // transform from specified reference frame to spacecraft frame
  113.             final StaticTransform refToSc = StaticTransform.compose(
  114.                     shifted,
  115.                     StaticTransform.of(
  116.                             shifted,
  117.                             pvProv.getPosition(shifted, frame).negate()),
  118.                     StaticTransform.of(
  119.                             shifted,
  120.                             attitudeLaw.getAttitudeRotation(pvProv, shifted, frame)));

  121.             // transform from specified reference frame to body frame
  122.             final StaticTransform refToBody;
  123.             if (i == 0) {
  124.                 refToBody = centralRefToBody = frame.getTransformTo(shape.getBodyFrame(), shifted);
  125.             } else {
  126.                 refToBody = frame.getStaticTransformTo(shape.getBodyFrame(), shifted);
  127.             }

  128.             sample.add(losIntersectionWithBody(StaticTransform.compose(shifted, refToSc.getInverse(), refToBody)));

  129.         }

  130.         // create interpolator
  131.         final TimeInterpolator<TimeStampedPVCoordinates> interpolator =
  132.                 new TimeStampedPVCoordinatesHermiteInterpolator(sample.size(), CartesianDerivativesFilter.USE_P);

  133.         // use interpolation to compute properly the time-derivatives
  134.         final TimeStampedPVCoordinates targetBody =
  135.                 interpolator.interpolate(date, sample);

  136.         // convert back to caller specified frame
  137.         return centralRefToBody.getInverse().transformPVCoordinates(targetBody);

  138.     }

  139.     /** {@inheritDoc} */
  140.     @Override
  141.     public <T extends CalculusFieldElement<T>> TimeStampedFieldPVCoordinates<T> getTargetPV(final FieldPVCoordinatesProvider<T> pvProv,
  142.                                                                                             final FieldAbsoluteDate<T> date,
  143.                                                                                             final Frame frame) {

  144.         // sample intersection points in current date neighborhood
  145.         final double h  = 0.1;
  146.         final List<TimeStampedFieldPVCoordinates<T>> sample = new ArrayList<>();
  147.         FieldTransform<T> centralRefToBody = null;
  148.         for (int i = -1; i < 2; ++i) {

  149.             final FieldAbsoluteDate<T> shifted = date.shiftedBy(i * h);

  150.             // transform from specified reference frame to spacecraft frame
  151.             final FieldStaticTransform<T> refToSc = FieldStaticTransform.compose(
  152.                     shifted,
  153.                     FieldStaticTransform.of(
  154.                             shifted,
  155.                             pvProv.getPVCoordinates(shifted, frame).getPosition().negate()),
  156.                     FieldStaticTransform.of(
  157.                             shifted,
  158.                             attitudeLaw.getAttitudeRotation(pvProv, shifted, frame)));

  159.             // transform from specified reference frame to body frame
  160.             final FieldStaticTransform<T> refToBody;
  161.             if (i == 0) {
  162.                 refToBody = centralRefToBody = frame.getTransformTo(shape.getBodyFrame(), shifted);
  163.             } else {
  164.                 refToBody = frame.getStaticTransformTo(shape.getBodyFrame(), shifted);
  165.             }

  166.             sample.add(losIntersectionWithBody(FieldStaticTransform.compose(shifted, refToSc.getInverse(), refToBody)));

  167.         }

  168.         // create interpolator
  169.         final FieldTimeInterpolator<TimeStampedFieldPVCoordinates<T>, T> interpolator =
  170.                 new TimeStampedFieldPVCoordinatesHermiteInterpolator<>(sample.size(), CartesianDerivativesFilter.USE_P);

  171.         // use interpolation to compute properly the time-derivatives
  172.         final TimeStampedFieldPVCoordinates<T> targetBody =
  173.                 interpolator.interpolate(date, sample);

  174.         // convert back to caller specified frame
  175.         return centralRefToBody.getInverse().transformPVCoordinates(targetBody);

  176.     }

  177.     /** Compute line of sight intersection with body.
  178.      * @param scToBody transform from spacecraft frame to body frame
  179.      * @return intersection point in body frame (only the position is set!)
  180.      */
  181.     private TimeStampedPVCoordinates losIntersectionWithBody(final StaticTransform scToBody) {

  182.         // compute satellite pointing axis and position/velocity in body frame
  183.         final Vector3D pointingBodyFrame = scToBody.transformVector(satPointingVector);
  184.         final Vector3D pBodyFrame        = scToBody.transformPosition(Vector3D.ZERO);

  185.         // Line from satellite following pointing direction
  186.         // we use arbitrarily the Earth radius as a scaling factor, it could be anything else
  187.         final Line pointingLine = new Line(pBodyFrame,
  188.                                            pBodyFrame.add(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
  189.                                                           pointingBodyFrame),
  190.                                            1.0e-10);

  191.         // Intersection with body shape
  192.         final GeodeticPoint gpIntersection =
  193.             shape.getIntersectionPoint(pointingLine, pBodyFrame, shape.getBodyFrame(), scToBody.getDate());
  194.         final Vector3D pIntersection =
  195.             (gpIntersection == null) ? null : shape.transform(gpIntersection);

  196.         // Check there is an intersection and it is not in the reverse pointing direction
  197.         if (pIntersection == null ||
  198.             Vector3D.dotProduct(pIntersection.subtract(pBodyFrame), pointingBodyFrame) < 0) {
  199.             throw new OrekitException(OrekitMessages.ATTITUDE_POINTING_LAW_DOES_NOT_POINT_TO_GROUND);
  200.         }

  201.         return new TimeStampedPVCoordinates(scToBody.getDate(),
  202.                                             pIntersection, Vector3D.ZERO, Vector3D.ZERO);

  203.     }

  204.     /** Compute line of sight intersection with body.
  205.      * @param scToBody transform from spacecraft frame to body frame
  206.      * @param <T> type of the field elements
  207.      * @return intersection point in body frame (only the position is set!)
  208.      */
  209.     private <T extends CalculusFieldElement<T>> TimeStampedFieldPVCoordinates<T> losIntersectionWithBody(final FieldStaticTransform<T> scToBody) {

  210.         // compute satellite pointing axis and position/velocity in body frame
  211.         final FieldVector3D<T> pointingBodyFrame = scToBody.transformVector(satPointingVector);
  212.         final FieldVector3D<T> pBodyFrame        = scToBody.transformPosition(Vector3D.ZERO);

  213.         // Line from satellite following pointing direction
  214.         // we use arbitrarily the Earth radius as a scaling factor, it could be anything else
  215.         final FieldLine<T> pointingLine = new FieldLine<>(pBodyFrame,
  216.                                                           pBodyFrame.add(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
  217.                                                                          pointingBodyFrame),
  218.                                                           1.0e-10);

  219.         // Intersection with body shape
  220.         final FieldGeodeticPoint<T> gpIntersection =
  221.             shape.getIntersectionPoint(pointingLine, pBodyFrame, shape.getBodyFrame(), new FieldAbsoluteDate<>(pBodyFrame.getX().getField(), scToBody.getDate()));
  222.         final FieldVector3D<T> pIntersection =
  223.             (gpIntersection == null) ? null : shape.transform(gpIntersection);

  224.         // Check there is an intersection and it is not in the reverse pointing direction
  225.         if (pIntersection == null ||
  226.             FieldVector3D.dotProduct(pIntersection.subtract(pBodyFrame), pointingBodyFrame).getReal() < 0) {
  227.             throw new OrekitException(OrekitMessages.ATTITUDE_POINTING_LAW_DOES_NOT_POINT_TO_GROUND);
  228.         }

  229.         final FieldVector3D<T> zero = FieldVector3D.getZero(pBodyFrame.getX().getField());
  230.         return new TimeStampedFieldPVCoordinates<>(scToBody.getDate(),
  231.                                                    pIntersection, zero, zero);

  232.     }

  233. }