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.utils;
18  
19  import org.hipparchus.analysis.differentiation.DerivativeStructure;
20  import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
21  import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
22  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
23  import org.hipparchus.geometry.euclidean.threed.Vector3D;
24  import org.junit.jupiter.api.Assertions;
25  import org.junit.jupiter.api.BeforeEach;
26  import org.junit.jupiter.api.Test;
27  import org.orekit.Utils;
28  import org.orekit.time.AbsoluteDate;
29  
30  public class TimeStampedPVCoordinatesTest {
31  
32      @Test
33      public void testPVOnlyConstructor() {
34          //setup
35          AbsoluteDate date = AbsoluteDate.J2000_EPOCH;
36          Vector3D p = new Vector3D(1, 2, 3);
37          Vector3D v = new Vector3D(4, 5, 6);
38  
39          //action
40          TimeStampedPVCoordinates actual = new TimeStampedPVCoordinates(date, p, v);
41  
42          //verify
43          Assertions.assertEquals(date, actual.getDate());
44          Assertions.assertEquals(1, actual.getPosition().getX(), 0);
45          Assertions.assertEquals(2, actual.getPosition().getY(), 0);
46          Assertions.assertEquals(3, actual.getPosition().getZ(), 0);
47          Assertions.assertEquals(4, actual.getVelocity().getX(), 0);
48          Assertions.assertEquals(5, actual.getVelocity().getY(), 0);
49          Assertions.assertEquals(6, actual.getVelocity().getZ(), 0);
50          Assertions.assertEquals(Vector3D.ZERO, actual.getAcceleration());
51      }
52  
53      @Test
54      public void testPVCoordinatesCopyConstructor() {
55          //setup
56          AbsoluteDate date = AbsoluteDate.J2000_EPOCH;
57          PVCoordinates pv = new PVCoordinates(new Vector3D(1, 2, 3), new Vector3D(4, 5, 6));
58  
59          //action
60          TimeStampedPVCoordinates actual = new TimeStampedPVCoordinates(date, pv);
61  
62          //verify
63          Assertions.assertEquals(date, actual.getDate());
64          Assertions.assertEquals(1, actual.getPosition().getX(), 0);
65          Assertions.assertEquals(2, actual.getPosition().getY(), 0);
66          Assertions.assertEquals(3, actual.getPosition().getZ(), 0);
67          Assertions.assertEquals(4, actual.getVelocity().getX(), 0);
68          Assertions.assertEquals(5, actual.getVelocity().getY(), 0);
69          Assertions.assertEquals(6, actual.getVelocity().getZ(), 0);
70          Assertions.assertEquals(Vector3D.ZERO, actual.getAcceleration());
71      }
72  
73      @Test
74      public void testLinearConstructors() {
75          TimeStampedPVCoordinates pv1 = new TimeStampedPVCoordinates(AbsoluteDate.CCSDS_EPOCH,
76                                                                      new Vector3D( 1,  0.1,   10),
77                                                                      new Vector3D(-1, -0.1,  -10),
78                                                                      new Vector3D(10, -1.0, -100));
79          TimeStampedPVCoordinates pv2 = new TimeStampedPVCoordinates(AbsoluteDate.FIFTIES_EPOCH,
80                                                                      new Vector3D( 2,  0.2,   20),
81                                                                      new Vector3D(-2, -0.2,  -20),
82                                                                      new Vector3D(20, -2.0, -200));
83          TimeStampedPVCoordinates pv3 = new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH,
84                                                                      new Vector3D( 3,  0.3,   30),
85                                                                      new Vector3D(-3, -0.3,  -30),
86                                                                      new Vector3D(30, -3.0, -300));
87          TimeStampedPVCoordinates pv4 = new TimeStampedPVCoordinates(AbsoluteDate.JULIAN_EPOCH,
88                                                                      new Vector3D( 4,  0.4,   40),
89                                                                      new Vector3D(-4, -0.4,  -40),
90                                                                      new Vector3D(40, -4.0, -400));
91          checkPV(pv4, new TimeStampedPVCoordinates(AbsoluteDate.JULIAN_EPOCH, 4, pv1), 1.0e-15);
92          checkPV(pv2, new TimeStampedPVCoordinates(AbsoluteDate.FIFTIES_EPOCH, pv1, pv3), 1.0e-15);
93          checkPV(pv3, new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH, 1, pv1, 1, pv2), 1.0e-15);
94          checkPV(new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH, 2, pv4),
95                  new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH, 3, pv1, 1, pv2, 1, pv3),
96                  1.0e-15);
97          checkPV(new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH, 3, pv3),
98                  new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH, 3, pv1, 1, pv2, 1, pv4),
99                  1.0e-15);
100         checkPV(new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH, 5, pv4),
101                 new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH, 4, pv1, 3, pv2, 2, pv3, 1, pv4),
102                 1.0e-15);
103     }
104 
105     @Test
106     public void testToDerivativeStructureVector1() {
107         FieldVector3D<DerivativeStructure> fv =
108                 new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH,
109                                              new Vector3D( 1,  0.1,  10),
110                                              new Vector3D(-1, -0.1, -10),
111                                              new Vector3D(10, -1.0, -100)).toDerivativeStructureVector(1);
112         Assertions.assertEquals(1, fv.getX().getFreeParameters());
113         Assertions.assertEquals(1, fv.getX().getOrder());
114         Assertions.assertEquals(   1.0, fv.getX().getReal(), 1.0e-10);
115         Assertions.assertEquals(   0.1, fv.getY().getReal(), 1.0e-10);
116         Assertions.assertEquals(  10.0, fv.getZ().getReal(), 1.0e-10);
117         Assertions.assertEquals(  -1.0, fv.getX().getPartialDerivative(1), 1.0e-15);
118         Assertions.assertEquals(  -0.1, fv.getY().getPartialDerivative(1), 1.0e-15);
119         Assertions.assertEquals( -10.0, fv.getZ().getPartialDerivative(1), 1.0e-15);
120         checkPV(new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH,
121                                              new Vector3D( 1,  0.1,  10),
122                                              new Vector3D(-1, -0.1, -10),
123                                              Vector3D.ZERO),
124                 new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH, fv), 1.0e-15);
125 
126         for (double dt = 0; dt < 10; dt += 0.125) {
127             Vector3D p = new PVCoordinates(new Vector3D( 1,  0.1,  10),
128                                            new Vector3D(-1, -0.1, -10)).shiftedBy(dt).getPosition();
129             Assertions.assertEquals(p.getX(), fv.getX().taylor(dt), 1.0e-14);
130             Assertions.assertEquals(p.getY(), fv.getY().taylor(dt), 1.0e-14);
131             Assertions.assertEquals(p.getZ(), fv.getZ().taylor(dt), 1.0e-14);
132         }
133 
134         TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH, fv);
135         Assertions.assertEquals(   1.0, pv.getPosition().getX(), 1.0e-10);
136         Assertions.assertEquals(   0.1, pv.getPosition().getY(), 1.0e-10);
137         Assertions.assertEquals(  10.0, pv.getPosition().getZ(), 1.0e-10);
138         Assertions.assertEquals(  -1.0, pv.getVelocity().getX(), 1.0e-15);
139         Assertions.assertEquals(  -0.1, pv.getVelocity().getY(), 1.0e-15);
140         Assertions.assertEquals( -10.0, pv.getVelocity().getZ(), 1.0e-15);
141 
142     }
143 
144     @Test
145     public void testToDerivativeStructureVector2() {
146         FieldVector3D<DerivativeStructure> fv =
147                 new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH,
148                                              new Vector3D( 1,  0.1,  10),
149                                              new Vector3D(-1, -0.1, -10),
150                                              new Vector3D(10, -1.0, -100)).toDerivativeStructureVector(2);
151         Assertions.assertEquals(1, fv.getX().getFreeParameters());
152         Assertions.assertEquals(2, fv.getX().getOrder());
153         Assertions.assertEquals(   1.0, fv.getX().getReal(), 1.0e-10);
154         Assertions.assertEquals(   0.1, fv.getY().getReal(), 1.0e-10);
155         Assertions.assertEquals(  10.0, fv.getZ().getReal(), 1.0e-10);
156         Assertions.assertEquals(  -1.0, fv.getX().getPartialDerivative(1), 1.0e-15);
157         Assertions.assertEquals(  -0.1, fv.getY().getPartialDerivative(1), 1.0e-15);
158         Assertions.assertEquals( -10.0, fv.getZ().getPartialDerivative(1), 1.0e-15);
159         Assertions.assertEquals(  10.0, fv.getX().getPartialDerivative(2), 1.0e-15);
160         Assertions.assertEquals(  -1.0, fv.getY().getPartialDerivative(2), 1.0e-15);
161         Assertions.assertEquals(-100.0, fv.getZ().getPartialDerivative(2), 1.0e-15);
162         checkPV(new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH,
163                                              new Vector3D( 1,  0.1,  10),
164                                              new Vector3D(-1, -0.1, -10),
165                                              new Vector3D(10, -1.0, -100)),
166                 new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH, fv), 1.0e-15);
167 
168         for (double dt = 0; dt < 10; dt += 0.125) {
169             Vector3D p = new PVCoordinates(new Vector3D( 1,  0.1,  10),
170                                            new Vector3D(-1, -0.1, -10),
171                                            new Vector3D(10, -1.0, -100)).shiftedBy(dt).getPosition();
172             Assertions.assertEquals(p.getX(), fv.getX().taylor(dt), 1.0e-14);
173             Assertions.assertEquals(p.getY(), fv.getY().taylor(dt), 1.0e-14);
174             Assertions.assertEquals(p.getZ(), fv.getZ().taylor(dt), 1.0e-14);
175         }
176 
177         TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH, fv);
178         Assertions.assertEquals(   1.0, pv.getPosition().getX(), 1.0e-10);
179         Assertions.assertEquals(   0.1, pv.getPosition().getY(), 1.0e-10);
180         Assertions.assertEquals(  10.0, pv.getPosition().getZ(), 1.0e-10);
181         Assertions.assertEquals(  -1.0, pv.getVelocity().getX(), 1.0e-15);
182         Assertions.assertEquals(  -0.1, pv.getVelocity().getY(), 1.0e-15);
183         Assertions.assertEquals( -10.0, pv.getVelocity().getZ(), 1.0e-15);
184         Assertions.assertEquals(  10.0, pv.getAcceleration().getX(), 1.0e-15);
185         Assertions.assertEquals(  -1.0, pv.getAcceleration().getY(), 1.0e-15);
186         Assertions.assertEquals(-100.0, pv.getAcceleration().getZ(), 1.0e-15);
187 
188     }
189 
190     @Test
191     public void testToUnivariateDerivative1Vector() {
192         FieldVector3D<UnivariateDerivative1> fv =
193                         new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH,
194                                                      new Vector3D( 1,  0.1,  10),
195                                                      new Vector3D(-1, -0.1, -10),
196                                                      new Vector3D(10, -1.0, -100)).toUnivariateDerivative1Vector();
197         Assertions.assertEquals(1, fv.getX().getFreeParameters());
198         Assertions.assertEquals(1, fv.getX().getOrder());
199         Assertions.assertEquals(   1.0, fv.getX().getReal(), 1.0e-10);
200         Assertions.assertEquals(   0.1, fv.getY().getReal(), 1.0e-10);
201         Assertions.assertEquals(  10.0, fv.getZ().getReal(), 1.0e-10);
202         Assertions.assertEquals(  -1.0, fv.getX().getPartialDerivative(1), 1.0e-15);
203         Assertions.assertEquals(  -0.1, fv.getY().getPartialDerivative(1), 1.0e-15);
204         Assertions.assertEquals( -10.0, fv.getZ().getPartialDerivative(1), 1.0e-15);
205         checkPV(new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH,
206                                              new Vector3D( 1,  0.1,  10),
207                                              new Vector3D(-1, -0.1, -10),
208                                              Vector3D.ZERO),
209                 new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH, fv), 1.0e-15);
210 
211         for (double dt = 0; dt < 10; dt += 0.125) {
212             Vector3D p = new PVCoordinates(new Vector3D( 1,  0.1,  10),
213                                            new Vector3D(-1, -0.1, -10)).shiftedBy(dt).getPosition();
214             Assertions.assertEquals(p.getX(), fv.getX().taylor(dt), 1.0e-14);
215             Assertions.assertEquals(p.getY(), fv.getY().taylor(dt), 1.0e-14);
216             Assertions.assertEquals(p.getZ(), fv.getZ().taylor(dt), 1.0e-14);
217         }
218 
219         TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH, fv);
220         Assertions.assertEquals(   1.0, pv.getPosition().getX(), 1.0e-10);
221         Assertions.assertEquals(   0.1, pv.getPosition().getY(), 1.0e-10);
222         Assertions.assertEquals(  10.0, pv.getPosition().getZ(), 1.0e-10);
223         Assertions.assertEquals(  -1.0, pv.getVelocity().getX(), 1.0e-15);
224         Assertions.assertEquals(  -0.1, pv.getVelocity().getY(), 1.0e-15);
225         Assertions.assertEquals( -10.0, pv.getVelocity().getZ(), 1.0e-15);
226 
227     }
228 
229     @Test
230     public void testToUnivariateDerivative2Vector() {
231         FieldVector3D<UnivariateDerivative2> fv =
232                         new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH,
233                                                      new Vector3D( 1,  0.1,  10),
234                                                      new Vector3D(-1, -0.1, -10),
235                                                      new Vector3D(10, -1.0, -100)).toUnivariateDerivative2Vector();
236         Assertions.assertEquals(1, fv.getX().getFreeParameters());
237         Assertions.assertEquals(2, fv.getX().getOrder());
238         Assertions.assertEquals(   1.0, fv.getX().getReal(), 1.0e-10);
239         Assertions.assertEquals(   0.1, fv.getY().getReal(), 1.0e-10);
240         Assertions.assertEquals(  10.0, fv.getZ().getReal(), 1.0e-10);
241         Assertions.assertEquals(  -1.0, fv.getX().getPartialDerivative(1), 1.0e-15);
242         Assertions.assertEquals(  -0.1, fv.getY().getPartialDerivative(1), 1.0e-15);
243         Assertions.assertEquals( -10.0, fv.getZ().getPartialDerivative(1), 1.0e-15);
244         Assertions.assertEquals(  10.0, fv.getX().getPartialDerivative(2), 1.0e-15);
245         Assertions.assertEquals(  -1.0, fv.getY().getPartialDerivative(2), 1.0e-15);
246         Assertions.assertEquals(-100.0, fv.getZ().getPartialDerivative(2), 1.0e-15);
247         checkPV(new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH,
248                                              new Vector3D( 1,  0.1,  10),
249                                              new Vector3D(-1, -0.1, -10),
250                                              new Vector3D(10, -1.0, -100)),
251                 new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH, fv), 1.0e-15);
252 
253         for (double dt = 0; dt < 10; dt += 0.125) {
254             Vector3D p = new PVCoordinates(new Vector3D( 1,  0.1,  10),
255                                            new Vector3D(-1, -0.1, -10),
256                                            new Vector3D(10, -1.0, -100)).shiftedBy(dt).getPosition();
257             Assertions.assertEquals(p.getX(), fv.getX().taylor(dt), 1.0e-14);
258             Assertions.assertEquals(p.getY(), fv.getY().taylor(dt), 1.0e-14);
259             Assertions.assertEquals(p.getZ(), fv.getZ().taylor(dt), 1.0e-14);
260         }
261 
262         TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(AbsoluteDate.GALILEO_EPOCH, fv);
263         Assertions.assertEquals(   1.0, pv.getPosition().getX(), 1.0e-10);
264         Assertions.assertEquals(   0.1, pv.getPosition().getY(), 1.0e-10);
265         Assertions.assertEquals(  10.0, pv.getPosition().getZ(), 1.0e-10);
266         Assertions.assertEquals(  -1.0, pv.getVelocity().getX(), 1.0e-15);
267         Assertions.assertEquals(  -0.1, pv.getVelocity().getY(), 1.0e-15);
268         Assertions.assertEquals( -10.0, pv.getVelocity().getZ(), 1.0e-15);
269         Assertions.assertEquals(  10.0, pv.getAcceleration().getX(), 1.0e-15);
270         Assertions.assertEquals(  -1.0, pv.getAcceleration().getY(), 1.0e-15);
271         Assertions.assertEquals(-100.0, pv.getAcceleration().getZ(), 1.0e-15);
272 
273     }
274 
275     @Test
276     public void testShift() {
277         Vector3D p1 = new Vector3D(  1,  0.1,   10);
278         Vector3D v1 = new Vector3D( -1, -0.1,  -10);
279         Vector3D a1 = new Vector3D( 10,  1.0,  100);
280         Vector3D p2 = new Vector3D(  7,  0.7,   70);
281         Vector3D v2 = new Vector3D(-11, -1.1, -110);
282         Vector3D a2 = new Vector3D( 10,  1.0,  100);
283         checkPV(new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH, p2, v2, a2),
284                 new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH.shiftedBy(1.0), p1, v1, a1).shiftedBy(-1.0), 1.0e-15);
285         Assertions.assertEquals(0.0, TimeStampedPVCoordinates.estimateVelocity(p1, p2, -1.0).subtract(new Vector3D(-6, -0.6, -60)).getNorm(), 1.0e-15);
286     }
287 
288     @Test
289     public void testToString() {
290         Utils.setDataRoot("regular-data");
291         TimeStampedPVCoordinates pv =
292             new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH,
293                                          new Vector3D( 1,   0.1,  10),
294                                          new Vector3D(-1,  -0.1, -10),
295                                          new Vector3D(10,   1.0, 100));
296         Assertions.assertEquals("{2000-01-01T11:58:55.816, P(1.0, 0.1, 10.0), V(-1.0, -0.1, -10.0), A(10.0, 1.0, 100.0)}", pv.toString());
297     }
298 
299     private void checkPV(TimeStampedPVCoordinates expected, TimeStampedPVCoordinates real, double epsilon) {
300         Assertions.assertEquals(expected.getDate(), real.getDate());
301         Assertions.assertEquals(expected.getPosition().getX(),     real.getPosition().getX(),     epsilon);
302         Assertions.assertEquals(expected.getPosition().getY(),     real.getPosition().getY(),     epsilon);
303         Assertions.assertEquals(expected.getPosition().getZ(),     real.getPosition().getZ(),     epsilon);
304         Assertions.assertEquals(expected.getVelocity().getX(),     real.getVelocity().getX(),     epsilon);
305         Assertions.assertEquals(expected.getVelocity().getY(),     real.getVelocity().getY(),     epsilon);
306         Assertions.assertEquals(expected.getVelocity().getZ(),     real.getVelocity().getZ(),     epsilon);
307         Assertions.assertEquals(expected.getAcceleration().getX(), real.getAcceleration().getX(), epsilon);
308         Assertions.assertEquals(expected.getAcceleration().getY(), real.getAcceleration().getY(), epsilon);
309         Assertions.assertEquals(expected.getAcceleration().getZ(), real.getAcceleration().getZ(), epsilon);
310     }
311 
312     @BeforeEach
313     public void setUp() {
314         Utils.setDataRoot("regular-data");
315     }
316 
317 }