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