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  
19  import java.util.ArrayList;
20  import java.util.List;
21  
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;
51  
52  /**
53   * This class handles nadir pointing attitude provider.
54  
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 {
65  
66      /** Body shape.  */
67      private final BodyShape shape;
68  
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      }
79  
80      /** {@inheritDoc} */
81      @Override
82      public TimeStampedPVCoordinates getTargetPV(final PVCoordinatesProvider pvProv,
83                                                  final AbsoluteDate date, final Frame frame) {
84  
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);
89  
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);
96  
97              final FieldVector3D<UnivariateDerivative2> positionInRefFrame = pvCoordinatesInRef.toUnivariateDerivative2Vector();
98              final FieldVector3D<UnivariateDerivative2> positionInBodyFrame = refToBody.transformPosition(positionInRefFrame);
99  
100             // satellite position in geodetic coordinates
101             final FieldGeodeticPoint<UnivariateDerivative2> gpSat = shape.transform(positionInBodyFrame, getBodyFrame(), ud2Date);
102 
103             // nadir position in geodetic coordinates
104             final FieldGeodeticPoint<UnivariateDerivative2> gpNadir = new FieldGeodeticPoint<>(gpSat.getLatitude(),
105                 gpSat.getLongitude(), ud2Field.getZero());
106 
107             // nadir point position in body frame
108             final FieldVector3D<UnivariateDerivative2> positionNadirInBodyFrame = shape.transform(gpNadir);
109 
110             // nadir point position in reference frame
111             final FieldStaticTransform<UnivariateDerivative2> bodyToRef = refToBody.getInverse();
112             final FieldVector3D<UnivariateDerivative2> positionNadirInRefFrame = bodyToRef.transformPosition(positionNadirInBodyFrame);
113 
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     }
122 
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) {
132 
133         // transform from specified reference frame to body frame
134         final Transform refToBody = frame.getTransformTo(shape.getBodyFrame(), date);
135 
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)));
144 
145         // create interpolator
146         final TimeInterpolator<TimeStampedPVCoordinates> interpolator =
147                 new TimeStampedPVCoordinatesHermiteInterpolator(sample.size(), CartesianDerivativesFilter.USE_P);
148 
149         // use interpolation to compute properly the time-derivatives
150         return interpolator.interpolate(date, sample);
151 
152     }
153 
154     /** {@inheritDoc} */
155     @Override
156     protected Vector3D getTargetPosition(final PVCoordinatesProvider pvProv, final AbsoluteDate date, final Frame frame) {
157 
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);
162 
163         return nadirRef(new TimeStampedPVCoordinates(date, pVWithoutDerivatives), refToBody).getPosition();
164     }
165 
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) {
171 
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);
177 
178         } else {  // use automatic differentiation
179             // build time dependent transform
180             final FieldUnivariateDerivative2Field<T> ud2Field = FieldUnivariateDerivative2Field.getUnivariateDerivative2Field(field);
181             final T shift = date.durationFrom(date.toAbsoluteDate());
182             final FieldUnivariateDerivative2<T> dt = new FieldUnivariateDerivative2<>(shift, field.getOne(), field.getZero());
183             final FieldAbsoluteDate<FieldUnivariateDerivative2<T>> ud2Date = new FieldAbsoluteDate<>(ud2Field, date.toAbsoluteDate()).shiftedBy(dt);
184             final FieldStaticTransform<FieldUnivariateDerivative2<T>> refToBody = frame.getStaticTransformTo(shape.getBodyFrame(), ud2Date);
185 
186             final FieldVector3D<FieldUnivariateDerivative2<T>> positionInRefFrame = pvCoordinatesInRef.toUnivariateDerivative2Vector();
187             final FieldVector3D<FieldUnivariateDerivative2<T>> positionInBodyFrame = refToBody.transformPosition(positionInRefFrame);
188 
189             // satellite position in geodetic coordinates
190             final FieldGeodeticPoint<FieldUnivariateDerivative2<T>> gpSat = shape.transform(positionInBodyFrame, getBodyFrame(), ud2Date);
191 
192             // nadir position in geodetic coordinates
193             final FieldGeodeticPoint<FieldUnivariateDerivative2<T>> gpNadir = new FieldGeodeticPoint<>(gpSat.getLatitude(),
194                     gpSat.getLongitude(), ud2Field.getZero());
195 
196             // nadir point position in body frame
197             final FieldVector3D<FieldUnivariateDerivative2<T>> positionNadirInBodyFrame = shape.transform(gpNadir);
198 
199             // nadir point position in reference frame
200             final FieldStaticTransform<FieldUnivariateDerivative2<T>> bodyToRef = refToBody.getInverse();
201             final FieldVector3D<FieldUnivariateDerivative2<T>> positionNadirInRefFrame = bodyToRef.transformPosition(positionNadirInBodyFrame);
202 
203             // put derivatives into proper object
204             final FieldVector3D<T> position = new FieldVector3D<>(positionNadirInRefFrame.getX().getValue(),
205                     positionNadirInRefFrame.getY().getValue(), positionNadirInRefFrame.getZ().getValue());
206             final FieldVector3D<T> velocity = new FieldVector3D<>(positionNadirInRefFrame.getX().getFirstDerivative(),
207                     positionNadirInRefFrame.getY().getFirstDerivative(), positionNadirInRefFrame.getZ().getFirstDerivative());
208             final FieldVector3D<T> acceleration = new FieldVector3D<>(positionNadirInRefFrame.getX().getSecondDerivative(),
209                     positionNadirInRefFrame.getY().getSecondDerivative(), positionNadirInRefFrame.getZ().getSecondDerivative());
210             return new TimeStampedFieldPVCoordinates<>(date, position, velocity, acceleration);
211         }
212 
213     }
214 
215     /**
216      * Compute target position-velocity-acceleration vector via interpolation (Field version).
217      * @param pvProv PV provider
218      * @param date date
219      * @param frame frame
220      * @param <T> field type
221      * @return target position-velocity-acceleration
222      */
223     public <T extends CalculusFieldElement<T>> TimeStampedFieldPVCoordinates<T> getTargetPVViaInterpolation(final FieldPVCoordinatesProvider<T> pvProv,
224                                                                                                             final FieldAbsoluteDate<T> date, final Frame frame) {
225 
226         // zero
227         final T zero = date.getField().getZero();
228 
229         // transform from specified reference frame to body frame
230         final FieldTransform<T> refToBody = frame.getTransformTo(shape.getBodyFrame(), date);
231 
232         // sample intersection points in current date neighborhood
233         final double h  = 0.01;
234         final List<TimeStampedFieldPVCoordinates<T>> sample = new ArrayList<>();
235         sample.add(nadirRef(pvProv.getPVCoordinates(date.shiftedBy(-2 * h), frame), refToBody.staticShiftedBy(zero.newInstance(-2 * h))));
236         sample.add(nadirRef(pvProv.getPVCoordinates(date.shiftedBy(-h),     frame), refToBody.staticShiftedBy(zero.newInstance(-h))));
237         sample.add(nadirRef(pvProv.getPVCoordinates(date,                   frame), refToBody));
238         sample.add(nadirRef(pvProv.getPVCoordinates(date.shiftedBy(+h),     frame), refToBody.staticShiftedBy(zero.newInstance(+h))));
239         sample.add(nadirRef(pvProv.getPVCoordinates(date.shiftedBy(+2 * h), frame), refToBody.staticShiftedBy(zero.newInstance(+2 * h))));
240 
241         // create interpolator
242         final FieldTimeInterpolator<TimeStampedFieldPVCoordinates<T>, T> interpolator =
243                 new TimeStampedFieldPVCoordinatesHermiteInterpolator<>(sample.size(), CartesianDerivativesFilter.USE_P);
244 
245         // use interpolation to compute properly the time-derivatives
246         return interpolator.interpolate(date, sample);
247 
248     }
249 
250     /** {@inheritDoc} */
251     @Override
252     protected <T extends CalculusFieldElement<T>> FieldVector3D<T> getTargetPosition(final FieldPVCoordinatesProvider<T> pvProv,
253                                                                                      final FieldAbsoluteDate<T> date,
254                                                                                      final Frame frame) {
255 
256         // transform from specified reference frame to body frame
257         final FieldVector3D<T> position = pvProv.getPosition(date, frame);
258         final FieldPVCoordinates<T> pVWithoutDerivatives = new FieldPVCoordinates<>(position, FieldVector3D.getZero(date.getField()));
259         final FieldStaticTransform<T> refToBody = frame.getStaticTransformTo(shape.getBodyFrame(), date);
260 
261         return nadirRef(new TimeStampedFieldPVCoordinates<T>(date, pVWithoutDerivatives), refToBody).getPosition();
262 
263     }
264 
265     /** Compute ground point in nadir direction, in reference frame.
266      * @param scRef spacecraft coordinates in reference frame
267      * @param refToBody transform from reference frame to body frame
268      * @return intersection point in body frame (only the position is set!)
269      */
270     private TimeStampedPVCoordinates nadirRef(final TimeStampedPVCoordinates scRef,
271                                               final StaticTransform refToBody) {
272 
273         final Vector3D satInBodyFrame = refToBody.transformPosition(scRef.getPosition());
274 
275         // satellite position in geodetic coordinates
276         final GeodeticPoint gpSat = shape.transform(satInBodyFrame, getBodyFrame(), scRef.getDate());
277 
278         // nadir position in geodetic coordinates
279         final GeodeticPoint gpNadir = new GeodeticPoint(gpSat.getLatitude(), gpSat.getLongitude(), 0.0);
280 
281         // nadir point position in body frame
282         final Vector3D pNadirBody = shape.transform(gpNadir);
283 
284         // nadir point position in reference frame
285         final Vector3D pNadirRef = refToBody.getInverse().transformPosition(pNadirBody);
286 
287         return new TimeStampedPVCoordinates(scRef.getDate(), pNadirRef, Vector3D.ZERO, Vector3D.ZERO);
288 
289     }
290 
291     /** Compute ground point in nadir direction, in reference frame.
292      * @param scRef spacecraft coordinates in reference frame
293      * @param refToBody transform from reference frame to body frame
294      * @param <T> type of the field elements
295      * @return intersection point in body frame (only the position is set!)
296      * @since 9.0
297      */
298     private <T extends CalculusFieldElement<T>> TimeStampedFieldPVCoordinates<T> nadirRef(final TimeStampedFieldPVCoordinates<T> scRef,
299                                                                                           final FieldStaticTransform<T> refToBody) {
300 
301         final FieldVector3D<T> satInBodyFrame = refToBody.transformPosition(scRef.getPosition());
302 
303         // satellite position in geodetic coordinates
304         final FieldGeodeticPoint<T> gpSat = shape.transform(satInBodyFrame, getBodyFrame(), scRef.getDate());
305 
306         // nadir position in geodetic coordinates
307         final FieldGeodeticPoint<T> gpNadir = new FieldGeodeticPoint<>(gpSat.getLatitude(), gpSat.getLongitude(),
308                                                                        gpSat.getAltitude().getField().getZero());
309 
310         // nadir point position in body frame
311         final FieldVector3D<T> pNadirBody = shape.transform(gpNadir);
312 
313         // nadir point position in reference frame
314         final FieldVector3D<T> pNadirRef = refToBody.getInverse().transformPosition(pNadirBody);
315 
316         final FieldVector3D<T> zero = FieldVector3D.getZero(gpSat.getAltitude().getField());
317         return new TimeStampedFieldPVCoordinates<>(scRef.getDate(), pNadirRef, zero, zero);
318 
319     }
320 
321 }