1   /* Copyright 2002-2025 Joseph Reed
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    * Joseph Reed 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.utils;
18  
19  import org.hipparchus.geometry.euclidean.threed.Vector3D;
20  import org.hipparchus.geometry.spherical.twod.Circle;
21  import org.hipparchus.geometry.spherical.twod.S2Point;
22  import org.hipparchus.util.FastMath;
23  import org.junit.jupiter.api.Assertions;
24  import org.junit.jupiter.api.BeforeEach;
25  import org.junit.jupiter.api.Test;
26  import org.orekit.Utils;
27  import org.orekit.bodies.GeodeticPoint;
28  import org.orekit.bodies.LoxodromeArc;
29  import org.orekit.bodies.OneAxisEllipsoid;
30  import org.orekit.errors.OrekitException;
31  import org.orekit.errors.OrekitMessages;
32  import org.orekit.frames.FramesFactory;
33  import org.orekit.frames.TopocentricFrame;
34  import org.orekit.models.earth.ReferenceEllipsoid;
35  import org.orekit.time.AbsoluteDate;
36  import org.orekit.time.TimeOffset;
37  
38  /** Unit tests for {@link WaypointPVBuilder}. */
39  public class WaypointPVBuilderTest {
40      /** epsilon for vector comparison. */
41      private static final double EPSILON = 1e7;
42  
43      /** Date of first waypoint. */
44      private AbsoluteDate date1;
45      /** Date of second waypoint. */
46      private AbsoluteDate date2;
47      /** Date of third waypoint. */
48      private AbsoluteDate date3;
49      /** New york. */
50      private GeodeticPoint newYork;
51      /** London. */
52      private GeodeticPoint london;
53      /** Philadelphia. */
54      private GeodeticPoint philadelphia;
55      /** Reference body (earth). */
56      private OneAxisEllipsoid body;
57  
58      /** Test setup. */
59      @BeforeEach
60      public void setUpBefore() {
61          Utils.setDataRoot("regular-data");
62  
63          date1 = AbsoluteDate.ARBITRARY_EPOCH;
64          date2 = date1.shiftedBy(3600.);
65          date3 = date2.shiftedBy(3600.);
66  
67          newYork = new GeodeticPoint(FastMath.toRadians(40.714268), FastMath.toRadians(-74.005974), 10.0584);
68          london = new GeodeticPoint(FastMath.toRadians(51.5), FastMath.toRadians(-0.16667), 10.9728);
69          philadelphia = new GeodeticPoint(FastMath.toRadians(39.952330), FastMath.toRadians(-75.16379), 11.8872);
70  
71          body = ReferenceEllipsoid.getWgs84(FramesFactory.getITRF(IERSConventions.IERS_2010, true));
72      }
73  
74      /** Verify cartesian interpolation. */
75      @Test
76      public void cartesian() {
77          final WaypointPVBuilder builder = WaypointPVBuilder.cartesianBuilder(body)
78                          .addWaypoint(philadelphia, date1)
79                          .addWaypoint(newYork, date2)
80                          .addWaypoint(london, date3)
81                          .invalidBefore()
82                          .invalidAfter();
83          final PVCoordinatesProvider pvProv = builder.build();
84  
85          final Vector3D expectedPhillyPos = body.transform(philadelphia);
86          final Vector3D expectedNyPos = body.transform(newYork);
87          final Vector3D expectedLondonPos = body.transform(london);
88  
89          final Vector3D segmentOneVel = expectedNyPos.subtract(expectedPhillyPos).scalarMultiply(1. / 3600.);
90          final Vector3D segmentTwoVel = expectedLondonPos.subtract(expectedNyPos).scalarMultiply(1. / 3600.);
91  
92          // check initial waypoint
93          final PVCoordinates pv1 = pvProv.getPVCoordinates(date1, body.getBodyFrame());
94          Assertions.assertEquals(expectedPhillyPos, pv1.getPosition());
95          Assertions.assertEquals(segmentOneVel, pv1.getVelocity());
96  
97          // check within segment 1
98          final PVCoordinates pv2 = pvProv.getPVCoordinates(date1.shiftedBy(900.), body.getBodyFrame());
99          Assertions.assertEquals(expectedPhillyPos.add(segmentOneVel.scalarMultiply(900.)), pv2.getPosition());
100         Assertions.assertEquals(segmentOneVel, pv2.getVelocity());
101 
102         // check second waypoint
103         final PVCoordinates pv3 = pvProv.getPVCoordinates(date2, body.getBodyFrame());
104         Assertions.assertEquals(expectedNyPos, pv3.getPosition());
105         Assertions.assertEquals(segmentTwoVel, pv3.getVelocity());
106 
107         // check within segment 2
108         final TimeStampedPVCoordinates pv4 = pvProv.getPVCoordinates(date2.shiftedBy(1800.), body.getBodyFrame());
109         Assertions.assertEquals(expectedNyPos.add(segmentTwoVel.scalarMultiply(1800.)),
110                                 pvProv.getPosition(pv4.getDate(), body.getBodyFrame()));
111         Assertions.assertEquals(segmentTwoVel, pv4.getVelocity());
112 
113         // check third waypoint
114         final PVCoordinates pv5 = pvProv.getPVCoordinates(date3, body.getBodyFrame());
115         Assertions.assertEquals(expectedLondonPos, pvProv.getPosition(date3, body.getBodyFrame()));
116         Assertions.assertEquals(Vector3D.ZERO, pv5.getVelocity());
117 
118         // check invalid before
119         try {
120             pvProv.getPVCoordinates(date1.shiftedBy(-1e-16), body.getBodyFrame());
121             Assertions.fail("expected exception, but none was thrown.");
122         } catch (final OrekitException ex) {
123             Assertions.assertEquals(OrekitMessages.OUT_OF_RANGE_DATE, ex.getSpecifier());
124         }
125         Vector3D pBefore = builder.constantBefore().build().getPosition(date1.shiftedBy(-60.0), body.getBodyFrame());
126         Assertions.assertEquals(0.0, Vector3D.distance(expectedPhillyPos, pBefore), 1.0e-15);
127 
128         // check invalid after
129         try {
130             pvProv.getPVCoordinates(date3.shiftedBy(new TimeOffset(2, TimeOffset.ATTOSECOND)), body.getBodyFrame());
131             Assertions.fail("expected exception, but none was thrown.");
132         } catch (final OrekitException ex) {
133             Assertions.assertEquals(OrekitMessages.OUT_OF_RANGE_DATE, ex.getSpecifier());
134         }
135         Vector3D pAfter = builder.constantAfter().build().getPosition(date3.shiftedBy(+60.0), body.getBodyFrame());
136         Assertions.assertEquals(0.0, Vector3D.distance(expectedLondonPos, pAfter), 1.0e-15);
137 
138     }
139 
140     /** Verify loxodrome interpolation. */
141     @Test
142     public void loxodrome() {
143         final PVCoordinatesProvider pvProv = WaypointPVBuilder.loxodromeBuilder(body)
144                 .addWaypoint(philadelphia, date1)
145                 .addWaypoint(newYork, date2)
146                 .addWaypoint(london, date3)
147                 .build();
148 
149         final Vector3D expectedPhillyPos = body.transform(philadelphia);
150         final Vector3D expectedNyPos = body.transform(newYork);
151         final Vector3D expectedLondonPos = body.transform(london);
152 
153         final LoxodromeArc arc1 = new LoxodromeArc(philadelphia, newYork, body);
154         final LoxodromeArc arc2 = new LoxodromeArc(newYork, london, body);
155 
156         final double vel1 = arc1.getDistance() / 3600.;
157         final double vel2 = arc2.getDistance() / 3600.;
158 
159         // check initial waypoint
160         final PVCoordinates pv1 = pvProv.getPVCoordinates(date1, body.getBodyFrame());
161         assertVectorsEqual(expectedPhillyPos, pv1.getPosition());
162         assertVectorsEqual(loxVelocity(arc1, expectedPhillyPos, vel1), pv1.getVelocity());
163 
164         // check within segment 1
165         final PVCoordinates pv2 = pvProv.getPVCoordinates(date1.shiftedBy(900.), body.getBodyFrame());
166         final Vector3D p2 = body.transform(arc1.calculatePointAlongArc(900. / 3600.));
167         assertVectorsEqual(p2, pv2.getPosition());
168         assertVectorsEqual(loxVelocity(arc1, p2, vel1), pv2.getVelocity());
169 
170         // check second waypoint
171         final PVCoordinates pv3 = pvProv.getPVCoordinates(date2, body.getBodyFrame());
172         assertVectorsEqual(expectedNyPos, pv3.getPosition());
173         assertVectorsEqual(loxVelocity(arc2, expectedNyPos, vel2), pv3.getVelocity());
174 
175         // check within segment 2
176         final TimeStampedPVCoordinates pv4 = pvProv.getPVCoordinates(date2.shiftedBy(1800.), body.getBodyFrame());
177         final Vector3D p4 = body.transform(arc2.calculatePointAlongArc(1800. / 3600.));
178         assertVectorsEqual(p4, pvProv.getPosition(pv4.getDate(),  body.getBodyFrame()));
179         assertVectorsEqual(loxVelocity(arc2, p4, vel2), pv4.getVelocity());
180 
181         // check third waypoint
182         final PVCoordinates pv5 = pvProv.getPVCoordinates(date3, body.getBodyFrame());
183         assertVectorsEqual(expectedLondonPos, pvProv.getPosition(date3, body.getBodyFrame()));
184         assertVectorsEqual(Vector3D.ZERO, pv5.getVelocity());
185 
186         // check invalid before
187         try {
188             pvProv.getPVCoordinates(date1.shiftedBy(-1e-16), body.getBodyFrame());
189             Assertions.fail("expected exception, but none was thrown.");
190         } catch (final OrekitException ex) {
191             Assertions.assertEquals(OrekitMessages.OUT_OF_RANGE_DATE, ex.getSpecifier());
192         }
193 
194         // check invalid after
195         try {
196             pvProv.getPVCoordinates(date3.shiftedBy(new TimeOffset(2, TimeOffset.ATTOSECOND)), body.getBodyFrame());
197             Assertions.fail("expected exception, but none was thrown.");
198         } catch (final OrekitException ex) {
199             Assertions.assertEquals(OrekitMessages.OUT_OF_RANGE_DATE, ex.getSpecifier());
200         }
201     }
202 
203     /** Verify great-circle interpolation. */
204     @Test
205     public void greatCircle() {
206         final PVCoordinatesProvider pvProv = WaypointPVBuilder.greatCircleBuilder(body)
207                 .addWaypoint(philadelphia, date1)
208                 .addWaypoint(newYork, date2)
209                 .addWaypoint(london, date3)
210                 .build();
211 
212         final Vector3D expectedPhillyPos = body.transform(philadelphia);
213         final Vector3D expectedNyPos = body.transform(newYork);
214         final Vector3D expectedLondonPos = body.transform(london);
215 
216         // check initial waypoint
217         final PVCoordinates pv1 = pvProv.getPVCoordinates(date1, body.getBodyFrame());
218         assertVectorsEqual(expectedPhillyPos, pv1.getPosition());
219         assertVectorsEqual(greatCircleVelocity(philadelphia, newYork, 0), pv1.getVelocity());
220 
221         // check within segment 1
222         final PVCoordinates pv2 = pvProv.getPVCoordinates(date1.shiftedBy(900.), body.getBodyFrame());
223         final Vector3D p2 = interpolateGreatCircle(philadelphia, newYork, 900. / 3600.);
224         assertVectorsEqual(p2, pv2.getPosition());
225         assertVectorsEqual(greatCircleVelocity(philadelphia, newYork, 900. / 3600.), pv2.getVelocity());
226 
227         // check second waypoint
228         final PVCoordinates pv3 = pvProv.getPVCoordinates(date2, body.getBodyFrame());
229         assertVectorsEqual(expectedNyPos, pv3.getPosition());
230         assertVectorsEqual(greatCircleVelocity(newYork, london, 0), pv3.getVelocity());
231 
232         // check within segment 2
233         final TimeStampedPVCoordinates pv4 = pvProv.getPVCoordinates(date2.shiftedBy(1800.), body.getBodyFrame());
234         final Vector3D p4 = interpolateGreatCircle(newYork, london, 1800. / 3600.);
235         assertVectorsEqual(p4, pvProv.getPosition(pv4.getDate(), body.getBodyFrame()));
236         assertVectorsEqual(greatCircleVelocity(newYork, london, 0.5), pv4.getVelocity());
237 
238         // check third waypoint
239         final PVCoordinates pv5 = pvProv.getPVCoordinates(date3, body.getBodyFrame());
240         assertVectorsEqual(expectedLondonPos, pvProv.getPosition(date3, body.getBodyFrame()));
241         assertVectorsEqual(Vector3D.ZERO, pv5.getVelocity());
242 
243         // check invalid before
244         try {
245             pvProv.getPVCoordinates(date1.shiftedBy(-1e-16), body.getBodyFrame());
246             Assertions.fail("expected exception, but none was thrown.");
247         } catch (final OrekitException ex) {
248             Assertions.assertEquals(OrekitMessages.OUT_OF_RANGE_DATE, ex.getSpecifier());
249         }
250 
251         // check invalid after
252         try {
253             pvProv.getPVCoordinates(date3.shiftedBy(new TimeOffset(2, TimeOffset.ATTOSECOND)), body.getBodyFrame());
254             Assertions.fail("expected exception, but none was thrown.");
255         } catch (final OrekitException ex) {
256             Assertions.assertEquals(OrekitMessages.OUT_OF_RANGE_DATE, ex.getSpecifier());
257         }
258     }
259 
260     private Vector3D loxVelocity(final LoxodromeArc arc, final Vector3D point, final double velMag) {
261         final GeodeticPoint p = arc.getBody().transform(point, arc.getBody().getBodyFrame(), AbsoluteDate.ARBITRARY_EPOCH);
262 
263         final Vector3D p2 = arc.getBody().transform(
264             new TopocentricFrame(body, p, "frame").pointAtDistance(arc.getAzimuth(), 0, velMag));
265         return p2.subtract(point);
266     }
267 
268     private static void assertVectorsEqual(final Vector3D expected, final Vector3D actual) {
269         if (Vector3D.distance(expected, actual) > EPSILON) {
270             Assertions.fail("Expected " + expected + " but was " + actual);
271         }
272     }
273 
274     private Vector3D interpolateGreatCircle(final GeodeticPoint p1, final GeodeticPoint p2, final double fraction) {
275         final S2Point sp1 = new S2Point(p1.getLongitude(), 0.5 * FastMath.PI - p1.getLatitude());
276         final S2Point sp2 = new S2Point(p2.getLongitude(), 0.5 * FastMath.PI - p2.getLatitude());
277 
278         final Circle c = new Circle(sp1, sp2, 1e-10);
279         final double phase1 = c.getPhase(sp1.getVector());
280         final double phase2 = c.getPhase(sp2.getVector());
281         final Vector3D pv = c.getPointAt(phase1 + (phase2 - phase1) * fraction);
282         final S2Point p = new S2Point(pv);
283 
284         final GeodeticPoint point = new GeodeticPoint(
285                 0.5 * FastMath.PI - p.getPhi(),
286                 p.getTheta(),
287                 p1.getAltitude() + (p2.getAltitude() + p1.getAltitude()) * fraction);
288         return body.transform(point);
289     }
290 
291     private Vector3D greatCircleVelocity(final GeodeticPoint p1, final GeodeticPoint p2, final double fraction) {
292         final S2Point sp1 = new S2Point(p1.getLongitude(), 0.5 * FastMath.PI - p1.getLatitude());
293         final S2Point sp2 = new S2Point(p2.getLongitude(), 0.5 * FastMath.PI - p2.getLatitude());
294 
295         final Circle c = new Circle(sp1, sp2, 1e-10);
296         final double phase1 = c.getPhase(sp1.getVector());
297         final double phase2 = c.getPhase(sp2.getVector());
298 
299         // compute the point at the fraction
300         final Vector3D pv = c.getPointAt(phase1 + (phase2 - phase1) * fraction);
301         final S2Point p = new S2Point(pv);
302 
303         final GeodeticPoint point = new GeodeticPoint(
304                 0.5 * FastMath.PI - p.getPhi(),
305                 p.getTheta(),
306                 p1.getAltitude() + (p2.getAltitude() + p1.getAltitude()) * fraction);
307 
308         // compute the point 1 second later
309         final double oneSecondPhase = (phase2 - phase1) / 3600.; // each segment is 1 hour long
310         final Vector3D pvPrime = c.getPointAt(phase1 + (phase2 - phase1) * fraction + oneSecondPhase);
311         final S2Point pPrime = new S2Point(pvPrime);
312 
313         final GeodeticPoint pointPrime = new GeodeticPoint(
314                 0.5 * FastMath.PI - pPrime.getPhi(),
315                 pPrime.getTheta(),
316                 p1.getAltitude() + (p2.getAltitude() + p1.getAltitude()) * (fraction + 1. / 3600.));
317         return body.transform(pointPrime).subtract(body.transform(point));
318     }
319 }