LofOffsetPointing.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.geometry.euclidean.threed.FieldLine;
  24. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  25. import org.hipparchus.geometry.euclidean.threed.Line;
  26. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  27. import org.hipparchus.geometry.euclidean.threed.Rotation;
  28. import org.hipparchus.geometry.euclidean.threed.FieldRotation;
  29. import org.orekit.bodies.BodyShape;
  30. import org.orekit.bodies.FieldGeodeticPoint;
  31. import org.orekit.bodies.GeodeticPoint;
  32. import org.orekit.errors.OrekitException;
  33. import org.orekit.errors.OrekitMessages;
  34. import org.orekit.frames.FieldStaticTransform;
  35. import org.orekit.frames.FieldTransform;
  36. import org.orekit.frames.Frame;
  37. import org.orekit.frames.StaticTransform;
  38. import org.orekit.frames.Transform;
  39. import org.orekit.time.AbsoluteDate;
  40. import org.orekit.time.FieldAbsoluteDate;
  41. import org.orekit.time.FieldTimeInterpolator;
  42. import org.orekit.time.TimeOffset;
  43. import org.orekit.time.TimeInterpolator;
  44. import org.orekit.utils.CartesianDerivativesFilter;
  45. import org.orekit.utils.Constants;
  46. import org.orekit.utils.FieldPVCoordinatesProvider;
  47. import org.orekit.utils.PVCoordinatesProvider;
  48. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  49. import org.orekit.utils.TimeStampedFieldPVCoordinatesHermiteInterpolator;
  50. import org.orekit.utils.TimeStampedPVCoordinates;
  51. import org.orekit.utils.TimeStampedPVCoordinatesHermiteInterpolator;

  53. /**
  54.  * This class provides a default attitude provider.

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

  64.     /** Rotation from local orbital frame. */
  65.     private final AttitudeProvider attitudeLaw;

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

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

  73.     /** Creates new instance.
  74.      * @param inertialFrame frame in which orbital velocities are computed
  75.      * @param shape Body shape
  76.      * @param attLaw Attitude law
  77.      * @param satPointingVector satellite vector defining the pointing direction
  78.      * @since 7.1
  79.      */
  80.     public LofOffsetPointing(final Frame inertialFrame, final BodyShape shape,
  81.                              final AttitudeProvider attLaw, final Vector3D satPointingVector) {
  82.         super(inertialFrame, shape.getBodyFrame());
  83.         this.shape = shape;
  84.         this.attitudeLaw = attLaw;
  85.         this.satPointingVector = satPointingVector;
  86.     }

  88.     /** {@inheritDoc} */
  89.     @Override
  90.     public Attitude getAttitude(final PVCoordinatesProvider pvProv,
  91.                                 final AbsoluteDate date, final Frame frame) {
  92.         return attitudeLaw.getAttitude(pvProv, date, frame);
  93.     }

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

  102.     /** {@inheritDoc} */
  103.     @Override
  104.     public Rotation getAttitudeRotation(final PVCoordinatesProvider pvProv,
  105.                                         final AbsoluteDate date, final Frame frame) {
  106.         return attitudeLaw.getAttitudeRotation(pvProv, date, frame);
  107.     }

  109.     /** {@inheritDoc} */
  110.     @Override
  111.     public <T extends CalculusFieldElement<T>> FieldRotation<T> getAttitudeRotation(final FieldPVCoordinatesProvider<T> pvProv,
  112.                                                                                     final FieldAbsoluteDate<T> date, final Frame frame) {
  113.         return attitudeLaw.getAttitudeRotation(pvProv, date, frame);
  114.     }

  116.     /** {@inheritDoc} */
  117.     @Override
  118.     public TimeStampedPVCoordinates getTargetPV(final PVCoordinatesProvider pvProv,
  119.                                                 final AbsoluteDate date, final Frame frame) {

  121.         // sample intersection points in current date neighborhood
  122.         final List<TimeStampedPVCoordinates> sample = new ArrayList<>();
  123.         Transform centralRefToBody = null;
  124.         for (int i = -1; i < 2; ++i) {

  126.             final AbsoluteDate shifted = date.shiftedBy(new TimeOffset(i * 100, TimeOffset.MILLISECOND));

  128.             // transform from specified reference frame to spacecraft frame
  129.             final StaticTransform refToSc = StaticTransform.of(shifted, pvProv.getPosition(shifted, frame).negate(),
  130.                 attitudeLaw.getAttitudeRotation(pvProv, shifted, frame));

  132.             // transform from specified reference frame to body frame
  133.             final StaticTransform refToBody;
  134.             if (i == 0) {
  135.                 refToBody = centralRefToBody = frame.getTransformTo(shape.getBodyFrame(), shifted);
  136.             } else {
  137.                 refToBody = frame.getStaticTransformTo(shape.getBodyFrame(), shifted);
  138.             }

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

  142.         }

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

  148.         // use interpolation to compute properly the time-derivatives
  149.         final TimeStampedPVCoordinates targetBody =
  150.                 interpolator.interpolate(date, sample);

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

  155.     }

  157.     /** {@inheritDoc} */
  158.     @Override
  159.     protected Vector3D getTargetPosition(final PVCoordinatesProvider pvProv, final AbsoluteDate date, final Frame frame) {

  161.         // transform from specified reference frame to spacecraft frame
  162.         final StaticTransform refToSc = StaticTransform.of(date, pvProv.getPosition(date, frame).negate(),
  163.             attitudeLaw.getAttitudeRotation(pvProv, date, frame));

  165.         // transform from specified reference frame to body frame
  166.         final StaticTransform refToBody = frame.getStaticTransformTo(shape.getBodyFrame(), date);
  167.         final Vector3D targetBody = losIntersectionWithBody(StaticTransform.compose(date, refToSc.getInverse(), refToBody)).getPosition();

  169.         // convert back to caller specified frame
  170.         return refToBody.getInverse().transformPosition(targetBody);
  171.     }

  173.     /** {@inheritDoc} */
  174.     @Override
  175.     public <T extends CalculusFieldElement<T>> TimeStampedFieldPVCoordinates<T> getTargetPV(final FieldPVCoordinatesProvider<T> pvProv,
  176.                                                                                             final FieldAbsoluteDate<T> date,
  177.                                                                                             final Frame frame) {

  179.         // sample intersection points in current date neighborhood
  180.         final List<TimeStampedFieldPVCoordinates<T>> sample = new ArrayList<>();
  181.         FieldTransform<T> centralRefToBody = null;
  182.         for (int i = -1; i < 2; ++i) {

  184.             final FieldAbsoluteDate<T> shifted = date.shiftedBy(new TimeOffset(i * 100, TimeOffset.MILLISECOND));

  186.             // transform from specified reference frame to spacecraft frame
  187.             final FieldStaticTransform<T> refToSc = FieldStaticTransform.of(shifted,
  188.                 pvProv.getPVCoordinates(shifted, frame).getPosition().negate(), attitudeLaw.getAttitudeRotation(pvProv, shifted, frame));

  190.             // transform from specified reference frame to body frame
  191.             final FieldStaticTransform<T> refToBody;
  192.             if (i == 0) {
  193.                 refToBody = centralRefToBody = frame.getTransformTo(shape.getBodyFrame(), shifted);
  194.             } else {
  195.                 refToBody = frame.getStaticTransformTo(shape.getBodyFrame(), shifted);
  196.             }

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

  200.         }

  202.         // create interpolator
  203.         final FieldTimeInterpolator<TimeStampedFieldPVCoordinates<T>, T> interpolator =
  204.                 new TimeStampedFieldPVCoordinatesHermiteInterpolator<>(sample.size(), CartesianDerivativesFilter.USE_P);

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

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

  213.     }

  215.     /** {@inheritDoc} */
  216.     @Override
  217.     protected <T extends CalculusFieldElement<T>> FieldVector3D<T> getTargetPosition(final FieldPVCoordinatesProvider<T> pvProv,
  218.                                                                                      final FieldAbsoluteDate<T> date,
  219.                                                                                      final Frame frame) {

  221.         // transform from specified reference frame to spacecraft frame
  222.         final FieldStaticTransform<T> refToSc = FieldStaticTransform.of(date, pvProv.getPosition(date, frame).negate(),
  223.                 attitudeLaw.getAttitudeRotation(pvProv, date, frame));

  225.         // transform from specified reference frame to body frame
  226.         final FieldStaticTransform<T> refToBody = frame.getStaticTransformTo(shape.getBodyFrame(), date);
  227.         final FieldVector3D<T> targetBody = losIntersectionWithBody(FieldStaticTransform.compose(date, refToSc.getInverse(), refToBody)).getPosition();

  229.         // convert back to caller specified frame
  230.         return refToBody.getInverse().transformPosition(targetBody);
  231.     }

  233.     /** Compute line of sight intersection with body.
  234.      * @param scToBody transform from spacecraft frame to body frame
  235.      * @return intersection point in body frame (only the position is set!)
  236.      */
  237.     private TimeStampedPVCoordinates losIntersectionWithBody(final StaticTransform scToBody) {

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

  243.         // Line from satellite following pointing direction
  244.         // we use arbitrarily the Earth radius as a scaling factor, it could be anything else
  245.         final Line pointingLine = new Line(pBodyFrame,
  246.                                            pBodyFrame.add(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
  247.                                                           pointingBodyFrame),
  248.                                            1.0e-10);

  250.         // Intersection with body shape
  251.         final GeodeticPoint gpIntersection =
  252.             shape.getIntersectionPoint(pointingLine, pBodyFrame, shape.getBodyFrame(), scToBody.getDate());
  253.         final Vector3D pIntersection =
  254.             (gpIntersection == null) ? null : shape.transform(gpIntersection);

  256.         // Check there is an intersection and it is not in the reverse pointing direction
  257.         if (pIntersection == null ||
  258.             Vector3D.dotProduct(pIntersection.subtract(pBodyFrame), pointingBodyFrame) < 0) {
  259.             throw new OrekitException(OrekitMessages.ATTITUDE_POINTING_LAW_DOES_NOT_POINT_TO_GROUND);
  260.         }

  262.         return new TimeStampedPVCoordinates(scToBody.getDate(),
  263.                                             pIntersection, Vector3D.ZERO, Vector3D.ZERO);

  265.     }

  267.     /** Compute line of sight intersection with body.
  268.      * @param scToBody transform from spacecraft frame to body frame
  269.      * @param <T> type of the field elements
  270.      * @return intersection point in body frame (only the position is set!)
  271.      */
  272.     private <T extends CalculusFieldElement<T>> TimeStampedFieldPVCoordinates<T> losIntersectionWithBody(final FieldStaticTransform<T> scToBody) {

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

  278.         // Line from satellite following pointing direction
  279.         // we use arbitrarily the Earth radius as a scaling factor, it could be anything else
  280.         final FieldLine<T> pointingLine = new FieldLine<>(pBodyFrame,
  281.                                                           pBodyFrame.add(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
  282.                                                                          pointingBodyFrame),
  283.                                                           1.0e-10);

  285.         // Intersection with body shape
  286.         final FieldGeodeticPoint<T> gpIntersection =
  287.             shape.getIntersectionPoint(pointingLine, pBodyFrame, shape.getBodyFrame(), new FieldAbsoluteDate<>(pBodyFrame.getX().getField(), scToBody.getDate()));
  288.         final FieldVector3D<T> pIntersection =
  289.             (gpIntersection == null) ? null : shape.transform(gpIntersection);

  291.         // Check there is an intersection and it is not in the reverse pointing direction
  292.         if (pIntersection == null ||
  293.             FieldVector3D.dotProduct(pIntersection.subtract(pBodyFrame), pointingBodyFrame).getReal() < 0) {
  294.             throw new OrekitException(OrekitMessages.ATTITUDE_POINTING_LAW_DOES_NOT_POINT_TO_GROUND);
  295.         }

  297.         final FieldVector3D<T> zero = FieldVector3D.getZero(pBodyFrame.getX().getField());
  298.         return new TimeStampedFieldPVCoordinates<>(scToBody.getDate(),
  299.                                                    pIntersection, zero, zero);

  301.     }

  303. }
Created with JaCoCo 0.8.12.202403310830