1   /* Copyright 2002-2022 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  
20  import org.hipparchus.CalculusFieldElement;
21  import org.hipparchus.analysis.UnivariateFunction;
22  import org.hipparchus.analysis.differentiation.DSFactory;
23  import org.hipparchus.analysis.differentiation.DerivativeStructure;
24  import org.hipparchus.analysis.differentiation.FieldDerivativeStructure;
25  import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative1;
26  import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
27  import org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator;
28  import org.hipparchus.analysis.interpolation.FieldHermiteInterpolator;
29  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
30  import org.hipparchus.geometry.euclidean.threed.Vector3D;
31  import org.hipparchus.random.RandomGenerator;
32  import org.hipparchus.random.Well19937a;
33  import org.hipparchus.util.Decimal64;
34  import org.hipparchus.util.Decimal64Field;
35  import org.hipparchus.util.FastMath;
36  import org.junit.Assert;
37  import org.junit.Test;
38  import org.orekit.errors.OrekitException;
39  import org.orekit.errors.OrekitMessages;
40  import org.orekit.frames.FramesFactory;
41  import org.orekit.orbits.FieldCartesianOrbit;
42  import org.orekit.time.FieldAbsoluteDate;
43  
44  
45  public class FieldPVCoordinatesTest {
46  
47      @Test
48      public void testLinearConstructors() {
49          DSFactory factory = new DSFactory(6, 1);
50          FieldPVCoordinates<DerivativeStructure> pv1 = new FieldPVCoordinates<>(createVector(1, 0.1, 10, 6),
51                                                                                 createVector(-1, -0.1, -10, 6));
52          FieldPVCoordinates<DerivativeStructure> pv2 = new FieldPVCoordinates<>(createVector(2, 0.2, 20, 6),
53                                                                                 createVector(-2, -0.2, -20, 6));
54          FieldPVCoordinates<DerivativeStructure> pv3 = new FieldPVCoordinates<>(createVector(3, 0.3, 30, 6),
55                                                                                 createVector(-3, -0.3, -30, 6));
56          FieldPVCoordinates<DerivativeStructure> pv4 = new FieldPVCoordinates<>(createVector(4, 0.4, 40, 6),
57                                                                                 createVector(-4, -0.4, -40, 6));
58          checkPV(pv4, new FieldPVCoordinates<>(4, pv1), 1.0e-15);
59          checkPV(pv4, new FieldPVCoordinates<>(factory.constant(4), pv1), 1.0e-15);
60          checkPV(pv4, new FieldPVCoordinates<>(factory.constant(4), pv1.toPVCoordinates()), 1.0e-15);
61          checkPV(pv2, new FieldPVCoordinates<>(pv1, pv3), 1.0e-15);
62          checkPV(pv3, new FieldPVCoordinates<>(1, pv1, 1, pv2), 1.0e-15);
63          checkPV(pv3, new FieldPVCoordinates<>(factory.constant(1), pv1,
64                                                factory.constant(1), pv2),
65                  1.0e-15);
66          checkPV(pv3, new FieldPVCoordinates<>(factory.constant(1), pv1.toPVCoordinates(),
67                                                factory.constant(1), pv2.toPVCoordinates()),
68                  1.0e-15);
69          checkPV(new FieldPVCoordinates<>(2, pv4), new FieldPVCoordinates<>(3, pv1, 1, pv2, 1, pv3), 1.0e-15);
70          checkPV(new FieldPVCoordinates<>(3, pv3), new FieldPVCoordinates<>(3, pv1, 1, pv2, 1, pv4), 1.0e-15);
71          checkPV(new FieldPVCoordinates<>(3, pv3),
72                  new FieldPVCoordinates<>(factory.constant(3), pv1,
73                                           factory.constant(1), pv2,
74                                           factory.constant(1), pv4),
75                  1.0e-15);
76          checkPV(new FieldPVCoordinates<>(3, pv3),
77                  new FieldPVCoordinates<>(factory.constant(3), pv1.toPVCoordinates(),
78                                           factory.constant(1), pv2.toPVCoordinates(),
79                                           factory.constant(1), pv4.toPVCoordinates()),
80                  1.0e-15);
81          checkPV(new FieldPVCoordinates<>(5, pv4),
82                  new FieldPVCoordinates<>(4, pv1, 3, pv2, 2, pv3, 1, pv4), 1.0e-15);
83          checkPV(new FieldPVCoordinates<>(5, pv4),
84                  new FieldPVCoordinates<>(factory.constant(4), pv1,
85                                           factory.constant(3), pv2,
86                                           factory.constant(2), pv3,
87                                           factory.constant(1), pv4),
88                  1.0e-15);
89          checkPV(new FieldPVCoordinates<>(5, pv4),
90                  new FieldPVCoordinates<>(factory.constant(4), pv1.toPVCoordinates(),
91                                           factory.constant(3), pv2.toPVCoordinates(),
92                                           factory.constant(2), pv3.toPVCoordinates(),
93                                           factory.constant(1), pv4.toPVCoordinates()),
94                  1.0e-15);
95      }
96  
97      @Test
98      public void testConversionConstructor() {
99          PVCoordinates pv = new PVCoordinates(new Vector3D(1, 2, 3), new Vector3D(4, 5, 6), new Vector3D(7, 8, 9));
100         FieldPVCoordinates<Decimal64> pv64 = new FieldPVCoordinates<>(Decimal64Field.getInstance(), pv);
101         Assert.assertEquals(0.0,
102                             Vector3D.distance(pv.getPosition(), pv64.getPosition().toVector3D()),
103                             1.0e-15);
104         Assert.assertEquals(0.0,
105                             Vector3D.distance(pv.getVelocity(), pv64.getVelocity().toVector3D()),
106                             1.0e-15);
107         Assert.assertEquals(0.0,
108                             Vector3D.distance(pv.getAcceleration(), pv64.getAcceleration().toVector3D()),
109                             1.0e-15);
110     }
111 
112     @Test
113     public void testZero() {
114         Assert.assertEquals(0.0,
115                             FieldPVCoordinates.getZero(Decimal64Field.getInstance()).getPosition().getNorm().getReal(),
116                             1.0e-15);
117         Assert.assertEquals(0.0,
118                             FieldPVCoordinates.getZero(Decimal64Field.getInstance()).getVelocity().getNorm().getReal(),
119                             1.0e-15);
120         Assert.assertEquals(0.0,
121                             FieldPVCoordinates.getZero(Decimal64Field.getInstance()).getAcceleration().getNorm().getReal(),
122                             1.0e-15);
123     }
124 
125     @Test
126     public void testGetMomentum() {
127         //setup
128         DSFactory factory = new DSFactory(1, 1);
129         DerivativeStructure oneDS = factory.getDerivativeField().getOne();
130         DerivativeStructure zeroDS = factory.getDerivativeField().getZero();
131         FieldVector3D<DerivativeStructure> zero = new FieldVector3D<>(zeroDS, zeroDS, zeroDS);
132         FieldVector3D<DerivativeStructure> i = new FieldVector3D<>(oneDS, zeroDS, zeroDS);
133         FieldVector3D<DerivativeStructure> j = new FieldVector3D<>(zeroDS, oneDS, zeroDS);
134         FieldVector3D<DerivativeStructure> k = new FieldVector3D<>(zeroDS, zeroDS, oneDS);
135         FieldVector3D<DerivativeStructure> p = new FieldVector3D<>(oneDS,
136                                                                    factory.constant(-2),
137                                                                    factory.constant(3));
138         FieldVector3D<DerivativeStructure> v = new FieldVector3D<>(factory.constant(-9),
139                                                                    factory.constant(8),
140                                                                    factory.constant(-7));
141 
142         //action + verify
143         Assert.assertEquals(
144                 new FieldPVCoordinates<>(p, v).getMomentum(),
145                 p.crossProduct(v));
146         //check simple cases
147         Assert.assertEquals(
148                 new FieldPVCoordinates<>(i, i.scalarMultiply(-1)).getMomentum(),
149                 zero);
150         Assert.assertEquals(
151                 new FieldPVCoordinates<>(i, j).getMomentum(),
152                 k);
153     }
154 
155     @Test
156     public void testGetAngularVelocity() {
157         //setup
158         DSFactory factory = new DSFactory(1, 1);
159         DerivativeStructure oneDS = factory.getDerivativeField().getOne();
160         DerivativeStructure zeroDS = factory.getDerivativeField().getZero();
161         FieldVector3D<DerivativeStructure> zero = new FieldVector3D<>(zeroDS, zeroDS, zeroDS);
162         FieldVector3D<DerivativeStructure> i = new FieldVector3D<>(oneDS, zeroDS, zeroDS);
163         FieldVector3D<DerivativeStructure> j = new FieldVector3D<>(zeroDS, oneDS, zeroDS);
164         FieldVector3D<DerivativeStructure> k = new FieldVector3D<>(zeroDS, zeroDS, oneDS);
165         FieldVector3D<DerivativeStructure> p = new FieldVector3D<>(oneDS,
166                                                                    factory.constant(-2),
167                                                                    factory.constant(3));
168         FieldVector3D<DerivativeStructure> v = new FieldVector3D<>(factory.constant(-9),
169                                                                    factory.constant(8),
170                                                                    factory.constant(-7));
171 
172         //action + verify
173         Assert.assertEquals(
174                 new FieldPVCoordinates<>(p, v).getAngularVelocity(),
175                 p.crossProduct(v).scalarMultiply(p.getNormSq().reciprocal()));
176         //check extra simple cases
177         Assert.assertEquals(
178                 new FieldPVCoordinates<>(i, i.scalarMultiply(-1)).getAngularVelocity(),
179                 zero);
180         Assert.assertEquals(
181                 new FieldPVCoordinates<>(i.scalarMultiply(2), j).getAngularVelocity(),
182                 k.scalarMultiply(0.5));
183     }
184 
185     @Test
186     public void testToDerivativeStructureVectorNeg() {
187         try {
188             FieldPVCoordinates.getZero(Decimal64Field.getInstance()).toDerivativeStructureVector(-1);
189             Assert.fail("an exception should have been thrown");
190         } catch (OrekitException oe) {
191             Assert.assertEquals(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, oe.getSpecifier());
192             Assert.assertEquals(-1, ((Integer) (oe.getParts()[0])).intValue());
193         }
194     }
195 
196     @Test
197     public void testToDerivativeStructureVector3() {
198         try {
199             FieldPVCoordinates.getZero(Decimal64Field.getInstance()).toDerivativeStructureVector(3);
200             Assert.fail("an exception should have been thrown");
201         } catch (OrekitException oe) {
202             Assert.assertEquals(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, oe.getSpecifier());
203             Assert.assertEquals(3, ((Integer) (oe.getParts()[0])).intValue());
204         }
205     }
206 
207     @Test
208     public void testToDerivativeStructureVector0() {
209         FieldVector3D<FieldDerivativeStructure<Decimal64>> fv =
210                 new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64( 1), new Decimal64( 0.1), new Decimal64( 10)),
211                                          new FieldVector3D<>(new Decimal64(-1), new Decimal64(-0.1), new Decimal64(-10)),
212                                          new FieldVector3D<>(new Decimal64(10), new Decimal64(-1.0), new Decimal64(-100))).
213                                          toDerivativeStructureVector(0);
214         Assert.assertEquals(1, fv.getX().getFreeParameters());
215         Assert.assertEquals(0, fv.getX().getOrder());
216         Assert.assertEquals(   1.0, fv.getX().getReal(), 1.0e-10);
217         Assert.assertEquals(   0.1, fv.getY().getReal(), 1.0e-10);
218         Assert.assertEquals(  10.0, fv.getZ().getReal(), 1.0e-10);
219         checkPV(new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64(1), new Decimal64(0.1), new Decimal64(10)),
220                                          FieldVector3D.getZero(Decimal64Field.getInstance()),
221                                          FieldVector3D.getZero(Decimal64Field.getInstance())),
222                 new FieldPVCoordinates<>(fv), 1.0e-15);
223     }
224 
225     @Test
226     public void testToDerivativeStructureVector1() {
227         FieldVector3D<FieldDerivativeStructure<Decimal64>> fv =
228                         new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64( 1), new Decimal64( 0.1), new Decimal64( 10)),
229                                                  new FieldVector3D<>(new Decimal64(-1), new Decimal64(-0.1), new Decimal64(-10)),
230                                                  new FieldVector3D<>(new Decimal64(10), new Decimal64(-1.0), new Decimal64(-100))).
231                                                  toDerivativeStructureVector(1);
232         Assert.assertEquals(1, fv.getX().getFreeParameters());
233         Assert.assertEquals(1, fv.getX().getOrder());
234         Assert.assertEquals(   1.0, fv.getX().getReal(), 1.0e-10);
235         Assert.assertEquals(   0.1, fv.getY().getReal(), 1.0e-10);
236         Assert.assertEquals(  10.0, fv.getZ().getReal(), 1.0e-10);
237         Assert.assertEquals(  -1.0, fv.getX().getPartialDerivative(1).getReal(), 1.0e-15);
238         Assert.assertEquals(  -0.1, fv.getY().getPartialDerivative(1).getReal(), 1.0e-15);
239         Assert.assertEquals( -10.0, fv.getZ().getPartialDerivative(1).getReal(), 1.0e-15);
240         checkPV(new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64(1), new Decimal64(0.1), new Decimal64(10)),
241                                          new FieldVector3D<>(new Decimal64(-1), new Decimal64(-0.1), new Decimal64(-10)),
242                                          FieldVector3D.getZero(Decimal64Field.getInstance())),
243                 new FieldPVCoordinates<>(fv), 1.0e-15);
244     }
245 
246     @Test
247     public void testUnivariateDerivative1Vector() {
248         FieldVector3D<FieldUnivariateDerivative1<Decimal64>> fv =
249                         new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64( 1), new Decimal64( 0.1), new Decimal64( 10)),
250                                                  new FieldVector3D<>(new Decimal64(-1), new Decimal64(-0.1), new Decimal64(-10)),
251                                                  new FieldVector3D<>(new Decimal64(10), new Decimal64(-1.0), new Decimal64(-100))).
252                                                  toUnivariateDerivative1Vector();
253         Assert.assertEquals(1, fv.getX().getOrder());
254         Assert.assertEquals(   1.0, fv.getX().getReal(), 1.0e-10);
255         Assert.assertEquals(   0.1, fv.getY().getReal(), 1.0e-10);
256         Assert.assertEquals(  10.0, fv.getZ().getReal(), 1.0e-10);
257         Assert.assertEquals(  -1.0, fv.getX().getDerivative(1).getReal(), 1.0e-15);
258         Assert.assertEquals(  -0.1, fv.getY().getDerivative(1).getReal(), 1.0e-15);
259         Assert.assertEquals( -10.0, fv.getZ().getDerivative(1).getReal(), 1.0e-15);
260 
261         FieldPVCoordinates<Decimal64> fpv = new FieldPVCoordinates<>(fv);
262         Assert.assertEquals(   1.0, fpv.getPosition().getX().getReal(), 1.0e-10);
263         Assert.assertEquals(   0.1, fpv.getPosition().getY().getReal(), 1.0e-10);
264         Assert.assertEquals(  10.0, fpv.getPosition().getZ().getReal(), 1.0e-10);
265         Assert.assertEquals(  -1.0, fpv.getVelocity().getX().getReal(), 1.0e-15);
266         Assert.assertEquals(  -0.1, fpv.getVelocity().getY().getReal(), 1.0e-15);
267         Assert.assertEquals( -10.0, fpv.getVelocity().getZ().getReal(), 1.0e-15);
268 
269     }
270 
271     @Test
272     public void testToDerivativeStructureVector2() {
273         FieldVector3D<FieldDerivativeStructure<Decimal64>> fv =
274                         new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64( 1), new Decimal64( 0.1), new Decimal64( 10)),
275                                                  new FieldVector3D<>(new Decimal64(-1), new Decimal64(-0.1), new Decimal64(-10)),
276                                                  new FieldVector3D<>(new Decimal64(10), new Decimal64(-1.0), new Decimal64(-100))).
277                                                  toDerivativeStructureVector(2);
278         Assert.assertEquals(1, fv.getX().getFreeParameters());
279         Assert.assertEquals(2, fv.getX().getOrder());
280         Assert.assertEquals(   1.0, fv.getX().getReal(), 1.0e-10);
281         Assert.assertEquals(   0.1, fv.getY().getReal(), 1.0e-10);
282         Assert.assertEquals(  10.0, fv.getZ().getReal(), 1.0e-10);
283         Assert.assertEquals(  -1.0, fv.getX().getPartialDerivative(1).getReal(), 1.0e-15);
284         Assert.assertEquals(  -0.1, fv.getY().getPartialDerivative(1).getReal(), 1.0e-15);
285         Assert.assertEquals( -10.0, fv.getZ().getPartialDerivative(1).getReal(), 1.0e-15);
286         Assert.assertEquals(  10.0, fv.getX().getPartialDerivative(2).getReal(), 1.0e-15);
287         Assert.assertEquals(  -1.0, fv.getY().getPartialDerivative(2).getReal(), 1.0e-15);
288         Assert.assertEquals(-100.0, fv.getZ().getPartialDerivative(2).getReal(), 1.0e-15);
289         checkPV(new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64(1), new Decimal64(0.1), new Decimal64(10)),
290                                          new FieldVector3D<>(new Decimal64(-1), new Decimal64(-0.1), new Decimal64(-10)),
291                                          new FieldVector3D<>(new Decimal64(10), new Decimal64(-1.0), new Decimal64(-100))),
292                 new FieldPVCoordinates<>(fv), 1.0e-15);
293 
294         for (double dt = 0; dt < 10; dt += 0.125) {
295             FieldVector3D<Decimal64> p = new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64( 1), new Decimal64( 0.1), new Decimal64( 10)),
296                                                                   new FieldVector3D<>(new Decimal64(-1), new Decimal64(-0.1), new Decimal64(-10)),
297                                                                   new FieldVector3D<>(new Decimal64(10), new Decimal64(-1.0), new Decimal64(-100))).
298                                          shiftedBy(dt).getPosition();
299             Assert.assertEquals(p.getX().doubleValue(), fv.getX().taylor(dt).doubleValue(), 1.0e-14);
300             Assert.assertEquals(p.getY().doubleValue(), fv.getY().taylor(dt).doubleValue(), 1.0e-14);
301             Assert.assertEquals(p.getZ().doubleValue(), fv.getZ().taylor(dt).doubleValue(), 1.0e-14);
302         }
303 
304         FieldPVCoordinates<Decimal64> fpv = new FieldPVCoordinates<>(fv);
305         Assert.assertEquals(   1.0, fpv.getPosition().getX().getReal(), 1.0e-10);
306         Assert.assertEquals(   0.1, fpv.getPosition().getY().getReal(), 1.0e-10);
307         Assert.assertEquals(  10.0, fpv.getPosition().getZ().getReal(), 1.0e-10);
308         Assert.assertEquals(  -1.0, fpv.getVelocity().getX().getReal(), 1.0e-15);
309         Assert.assertEquals(  -0.1, fpv.getVelocity().getY().getReal(), 1.0e-15);
310         Assert.assertEquals( -10.0, fpv.getVelocity().getZ().getReal(), 1.0e-15);
311         Assert.assertEquals(  10.0, fpv.getAcceleration().getX().getReal(), 1.0e-15);
312         Assert.assertEquals(  -1.0, fpv.getAcceleration().getY().getReal(), 1.0e-15);
313         Assert.assertEquals(-100.0, fpv.getAcceleration().getZ().getReal(), 1.0e-15);
314 
315     }
316 
317     @Test
318     public void testUnivariateDerivative2Vector() {
319         FieldVector3D<FieldUnivariateDerivative2<Decimal64>> fv =
320                         new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64( 1),  new Decimal64(0.1),  new Decimal64(10)),
321                                                  new FieldVector3D<>(new Decimal64(-1), new Decimal64(-0.1), new Decimal64(-10)),
322                                                  new FieldVector3D<>(new Decimal64(10), new Decimal64(-1.0), new Decimal64(-100))).toUnivariateDerivative2Vector();
323         Assert.assertEquals(2, fv.getX().getOrder());
324         Assert.assertEquals(   1.0, fv.getX().getReal(), 1.0e-10);
325         Assert.assertEquals(   0.1, fv.getY().getReal(), 1.0e-10);
326         Assert.assertEquals(  10.0, fv.getZ().getReal(), 1.0e-10);
327         Assert.assertEquals(  -1.0, fv.getX().getDerivative(1).getReal(), 1.0e-15);
328         Assert.assertEquals(  -0.1, fv.getY().getDerivative(1).getReal(), 1.0e-15);
329         Assert.assertEquals( -10.0, fv.getZ().getDerivative(1).getReal(), 1.0e-15);
330         Assert.assertEquals(  10.0, fv.getX().getDerivative(2).getReal(), 1.0e-15);
331         Assert.assertEquals(  -1.0, fv.getY().getDerivative(2).getReal(), 1.0e-15);
332         Assert.assertEquals(-100.0, fv.getZ().getDerivative(2).getReal(), 1.0e-15);
333 
334         for (double dt = 0; dt < 10; dt += 0.125) {
335             FieldVector3D<Decimal64> p = new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64( 1),  new Decimal64(0.1),  new Decimal64(10)),
336                                                                   new FieldVector3D<>(new Decimal64(-1), new Decimal64(-0.1), new Decimal64(-10)),
337                                                                   new FieldVector3D<>(new Decimal64(10), new Decimal64(-1.0), new Decimal64(-100))).shiftedBy(dt).getPosition();
338             Assert.assertEquals(p.getX().doubleValue(), fv.getX().taylor(dt).doubleValue(), 1.0e-14);
339             Assert.assertEquals(p.getY().doubleValue(), fv.getY().taylor(dt).doubleValue(), 1.0e-14);
340             Assert.assertEquals(p.getZ().doubleValue(), fv.getZ().taylor(dt).doubleValue(), 1.0e-14);
341         }
342 
343         FieldPVCoordinates<Decimal64> fpv = new FieldPVCoordinates<>(fv);
344         Assert.assertEquals(   1.0, fpv.getPosition().getX().getReal(), 1.0e-10);
345         Assert.assertEquals(   0.1, fpv.getPosition().getY().getReal(), 1.0e-10);
346         Assert.assertEquals(  10.0, fpv.getPosition().getZ().getReal(), 1.0e-10);
347         Assert.assertEquals(  -1.0, fpv.getVelocity().getX().getReal(), 1.0e-15);
348         Assert.assertEquals(  -0.1, fpv.getVelocity().getY().getReal(), 1.0e-15);
349         Assert.assertEquals( -10.0, fpv.getVelocity().getZ().getReal(), 1.0e-15);
350         Assert.assertEquals(  10.0, fpv.getAcceleration().getX().getReal(), 1.0e-15);
351         Assert.assertEquals(  -1.0, fpv.getAcceleration().getY().getReal(), 1.0e-15);
352         Assert.assertEquals(-100.0, fpv.getAcceleration().getZ().getReal(), 1.0e-15);
353 
354     }
355 
356     @Test
357     public void testToDerivativeStructurePVNeg() {
358         try {
359             FieldPVCoordinates.getZero(Decimal64Field.getInstance()).toDerivativeStructurePV(-1);
360             Assert.fail("an exception should have been thrown");
361         } catch (OrekitException oe) {
362             Assert.assertEquals(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, oe.getSpecifier());
363             Assert.assertEquals(-1, ((Integer) (oe.getParts()[0])).intValue());
364         }
365     }
366 
367     @Test
368     public void testToDerivativeStructurePV3() {
369         try {
370             FieldPVCoordinates.getZero(Decimal64Field.getInstance()).toDerivativeStructurePV(3);
371             Assert.fail("an exception should have been thrown");
372         } catch (OrekitException oe) {
373             Assert.assertEquals(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, oe.getSpecifier());
374             Assert.assertEquals(3, ((Integer) (oe.getParts()[0])).intValue());
375         }
376     }
377 
378     @Test
379     public void testToDerivativeStructurePV0() {
380         FieldPVCoordinates<DerivativeStructure> fv =
381                 new PVCoordinates(new Vector3D( 1,  0.1,  10),
382                                   new Vector3D(-1, -0.1, -10),
383                                   new Vector3D(10, -1.0, -100)).toDerivativeStructurePV(0);
384         Assert.assertEquals(1, fv.getPosition().getX().getFreeParameters());
385         Assert.assertEquals(0, fv.getPosition().getX().getOrder());
386         Assert.assertEquals(   1.0, fv.getPosition().getX().getReal(),     1.0e-10);
387         Assert.assertEquals(   0.1, fv.getPosition().getY().getReal(),     1.0e-10);
388         Assert.assertEquals(  10.0, fv.getPosition().getZ().getReal(),     1.0e-10);
389         Assert.assertEquals(  -1.0, fv.getVelocity().getX().getReal(),     1.0e-10);
390         Assert.assertEquals(  -0.1, fv.getVelocity().getY().getReal(),     1.0e-10);
391         Assert.assertEquals( -10.0, fv.getVelocity().getZ().getReal(),     1.0e-10);
392         Assert.assertEquals(  10.0, fv.getAcceleration().getX().getReal(), 1.0e-10);
393         Assert.assertEquals(  -1.0, fv.getAcceleration().getY().getReal(), 1.0e-10);
394         Assert.assertEquals(-100.0, fv.getAcceleration().getZ().getReal(), 1.0e-10);
395     }
396 
397     @Test
398     public void testToDerivativeStructurePV1() {
399         FieldPVCoordinates<FieldDerivativeStructure<Decimal64>> fv =
400                         new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64( 1), new Decimal64( 0.1), new Decimal64( 10)),
401                                                  new FieldVector3D<>(new Decimal64(-1), new Decimal64(-0.1), new Decimal64(-10)),
402                                                  new FieldVector3D<>(new Decimal64(10), new Decimal64(-1.0), new Decimal64(-100))).
403                                                  toDerivativeStructurePV(1);
404         Assert.assertEquals(1, fv.getPosition().getX().getFreeParameters());
405         Assert.assertEquals(1, fv.getPosition().getX().getOrder());
406         Assert.assertEquals(   1.0, fv.getPosition().getX().getReal(),     1.0e-10);
407         Assert.assertEquals(   0.1, fv.getPosition().getY().getReal(),     1.0e-10);
408         Assert.assertEquals(  10.0, fv.getPosition().getZ().getReal(),     1.0e-10);
409         Assert.assertEquals(  -1.0, fv.getVelocity().getX().getReal(),     1.0e-10);
410         Assert.assertEquals(  -0.1, fv.getVelocity().getY().getReal(),     1.0e-10);
411         Assert.assertEquals( -10.0, fv.getVelocity().getZ().getReal(),     1.0e-10);
412         Assert.assertEquals(  10.0, fv.getAcceleration().getX().getReal(), 1.0e-10);
413         Assert.assertEquals(  -1.0, fv.getAcceleration().getY().getReal(), 1.0e-10);
414         Assert.assertEquals(-100.0, fv.getAcceleration().getZ().getReal(), 1.0e-10);
415 
416         Assert.assertEquals(fv.getVelocity().getX().getReal(),     fv.getPosition().getX().getPartialDerivative(1).getReal(), 1.0e-10);
417         Assert.assertEquals(fv.getVelocity().getY().getReal(),     fv.getPosition().getY().getPartialDerivative(1).getReal(), 1.0e-10);
418         Assert.assertEquals(fv.getVelocity().getZ().getReal(),     fv.getPosition().getZ().getPartialDerivative(1).getReal(), 1.0e-10);
419         Assert.assertEquals(fv.getAcceleration().getX().getReal(), fv.getVelocity().getX().getPartialDerivative(1).getReal(), 1.0e-10);
420         Assert.assertEquals(fv.getAcceleration().getY().getReal(), fv.getVelocity().getY().getPartialDerivative(1).getReal(), 1.0e-10);
421         Assert.assertEquals(fv.getAcceleration().getZ().getReal(), fv.getVelocity().getZ().getPartialDerivative(1).getReal(), 1.0e-10);
422 
423     }
424 
425 
426     @Test
427     public void testToUnivariateDerivative1PV() {
428         FieldPVCoordinates<FieldUnivariateDerivative1<Decimal64>> fv =
429                         new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64( 1), new Decimal64( 0.1), new Decimal64( 10)),
430                                                  new FieldVector3D<>(new Decimal64(-1), new Decimal64(-0.1), new Decimal64(-10)),
431                                                  new FieldVector3D<>(new Decimal64(10), new Decimal64(-1.0), new Decimal64(-100))).
432                                                  toUnivariateDerivative1PV();
433         Assert.assertEquals(1, fv.getPosition().getX().getOrder());
434         Assert.assertEquals(   1.0, fv.getPosition().getX().getReal(),     1.0e-10);
435         Assert.assertEquals(   0.1, fv.getPosition().getY().getReal(),     1.0e-10);
436         Assert.assertEquals(  10.0, fv.getPosition().getZ().getReal(),     1.0e-10);
437         Assert.assertEquals(  -1.0, fv.getVelocity().getX().getReal(),     1.0e-10);
438         Assert.assertEquals(  -0.1, fv.getVelocity().getY().getReal(),     1.0e-10);
439         Assert.assertEquals( -10.0, fv.getVelocity().getZ().getReal(),     1.0e-10);
440         Assert.assertEquals(  10.0, fv.getAcceleration().getX().getReal(), 1.0e-10);
441         Assert.assertEquals(  -1.0, fv.getAcceleration().getY().getReal(), 1.0e-10);
442         Assert.assertEquals(-100.0, fv.getAcceleration().getZ().getReal(), 1.0e-10);
443 
444         Assert.assertEquals(fv.getVelocity().getX().getReal(),     fv.getPosition().getX().getDerivative(1).getReal(), 1.0e-10);
445         Assert.assertEquals(fv.getVelocity().getY().getReal(),     fv.getPosition().getY().getDerivative(1).getReal(), 1.0e-10);
446         Assert.assertEquals(fv.getVelocity().getZ().getReal(),     fv.getPosition().getZ().getDerivative(1).getReal(), 1.0e-10);
447         Assert.assertEquals(fv.getAcceleration().getX().getReal(), fv.getVelocity().getX().getDerivative(1).getReal(), 1.0e-10);
448         Assert.assertEquals(fv.getAcceleration().getY().getReal(), fv.getVelocity().getY().getDerivative(1).getReal(), 1.0e-10);
449         Assert.assertEquals(fv.getAcceleration().getZ().getReal(), fv.getVelocity().getZ().getDerivative(1).getReal(), 1.0e-10);
450     }
451 
452     @Test
453     public void testToDerivativeStructurePV2() {
454         FieldPVCoordinates<FieldDerivativeStructure<Decimal64>> fv =
455                         new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64( 1), new Decimal64( 0.1), new Decimal64( 10)),
456                                                  new FieldVector3D<>(new Decimal64(-1), new Decimal64(-0.1), new Decimal64(-10)),
457                                                  new FieldVector3D<>(new Decimal64(10), new Decimal64(-1.0), new Decimal64(-100))).
458                                                  toDerivativeStructurePV(2);
459         Assert.assertEquals(1, fv.getPosition().getX().getFreeParameters());
460         Assert.assertEquals(2, fv.getPosition().getX().getOrder());
461         Assert.assertEquals(   1.0, fv.getPosition().getX().getReal(),     1.0e-10);
462         Assert.assertEquals(   0.1, fv.getPosition().getY().getReal(),     1.0e-10);
463         Assert.assertEquals(  10.0, fv.getPosition().getZ().getReal(),     1.0e-10);
464         Assert.assertEquals(  -1.0, fv.getVelocity().getX().getReal(),     1.0e-10);
465         Assert.assertEquals(  -0.1, fv.getVelocity().getY().getReal(),     1.0e-10);
466         Assert.assertEquals( -10.0, fv.getVelocity().getZ().getReal(),     1.0e-10);
467         Assert.assertEquals(  10.0, fv.getAcceleration().getX().getReal(), 1.0e-10);
468         Assert.assertEquals(  -1.0, fv.getAcceleration().getY().getReal(), 1.0e-10);
469         Assert.assertEquals(-100.0, fv.getAcceleration().getZ().getReal(), 1.0e-10);
470 
471         Assert.assertEquals(fv.getVelocity().getX().getReal(),                             fv.getPosition().getX().getPartialDerivative(1).getReal(), 1.0e-10);
472         Assert.assertEquals(fv.getVelocity().getY().getReal(),                             fv.getPosition().getY().getPartialDerivative(1).getReal(), 1.0e-10);
473         Assert.assertEquals(fv.getVelocity().getZ().getReal(),                             fv.getPosition().getZ().getPartialDerivative(1).getReal(), 1.0e-10);
474         Assert.assertEquals(fv.getAcceleration().getX().getReal(),                         fv.getPosition().getX().getPartialDerivative(2).getReal(), 1.0e-10);
475         Assert.assertEquals(fv.getAcceleration().getY().getReal(),                         fv.getPosition().getY().getPartialDerivative(2).getReal(), 1.0e-10);
476         Assert.assertEquals(fv.getAcceleration().getZ().getReal(),                         fv.getPosition().getZ().getPartialDerivative(2).getReal(), 1.0e-10);
477         Assert.assertEquals(fv.getAcceleration().getX().getReal(),                         fv.getVelocity().getX().getPartialDerivative(1).getReal(), 1.0e-10);
478         Assert.assertEquals(fv.getAcceleration().getY().getReal(),                         fv.getVelocity().getY().getPartialDerivative(1).getReal(), 1.0e-10);
479         Assert.assertEquals(fv.getAcceleration().getZ().getReal(),                         fv.getVelocity().getZ().getPartialDerivative(1).getReal(), 1.0e-10);
480         Assert.assertEquals(fv.getAcceleration().getX().getPartialDerivative(1).getReal(), fv.getVelocity().getX().getPartialDerivative(2).getReal(), 1.0e-10);
481         Assert.assertEquals(fv.getAcceleration().getY().getPartialDerivative(1).getReal(), fv.getVelocity().getY().getPartialDerivative(2).getReal(), 1.0e-10);
482         Assert.assertEquals(fv.getAcceleration().getZ().getPartialDerivative(1).getReal(), fv.getVelocity().getZ().getPartialDerivative(2).getReal(), 1.0e-10);
483 
484         for (double dt = 0; dt < 10; dt += 0.125) {
485             Vector3D p = new PVCoordinates(new Vector3D( 1,  0.1,  10),
486                                            new Vector3D(-1, -0.1, -10),
487                                            new Vector3D(10, -1.0, -100)).shiftedBy(dt).getPosition();
488             Assert.assertEquals(p.getX(), fv.getPosition().getX().taylor(dt).getReal(), 1.0e-14);
489             Assert.assertEquals(p.getY(), fv.getPosition().getY().taylor(dt).getReal(), 1.0e-14);
490             Assert.assertEquals(p.getZ(), fv.getPosition().getZ().taylor(dt).getReal(), 1.0e-14);
491         }
492 
493     }
494 
495     @Test
496     public void testToUnivariateDerivative2PV() {
497         FieldPVCoordinates<FieldUnivariateDerivative2<Decimal64>> fv =
498                         new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64( 1), new Decimal64( 0.1), new Decimal64( 10)),
499                                                  new FieldVector3D<>(new Decimal64(-1), new Decimal64(-0.1), new Decimal64(-10)),
500                                                  new FieldVector3D<>(new Decimal64(10), new Decimal64(-1.0), new Decimal64(-100))).
501                                                  toUnivariateDerivative2PV();
502         Assert.assertEquals(2, fv.getPosition().getX().getOrder());
503         Assert.assertEquals(   1.0, fv.getPosition().getX().getReal(),     1.0e-10);
504         Assert.assertEquals(   0.1, fv.getPosition().getY().getReal(),     1.0e-10);
505         Assert.assertEquals(  10.0, fv.getPosition().getZ().getReal(),     1.0e-10);
506         Assert.assertEquals(  -1.0, fv.getVelocity().getX().getReal(),     1.0e-10);
507         Assert.assertEquals(  -0.1, fv.getVelocity().getY().getReal(),     1.0e-10);
508         Assert.assertEquals( -10.0, fv.getVelocity().getZ().getReal(),     1.0e-10);
509         Assert.assertEquals(  10.0, fv.getAcceleration().getX().getReal(), 1.0e-10);
510         Assert.assertEquals(  -1.0, fv.getAcceleration().getY().getReal(), 1.0e-10);
511         Assert.assertEquals(-100.0, fv.getAcceleration().getZ().getReal(), 1.0e-10);
512 
513         Assert.assertEquals(fv.getVelocity().getX().getReal(),                      fv.getPosition().getX().getDerivative(1).getReal(), 1.0e-10);
514         Assert.assertEquals(fv.getVelocity().getY().getReal(),                      fv.getPosition().getY().getDerivative(1).getReal(), 1.0e-10);
515         Assert.assertEquals(fv.getVelocity().getZ().getReal(),                      fv.getPosition().getZ().getDerivative(1).getReal(), 1.0e-10);
516         Assert.assertEquals(fv.getAcceleration().getX().getReal(),                  fv.getPosition().getX().getDerivative(2).getReal(), 1.0e-10);
517         Assert.assertEquals(fv.getAcceleration().getY().getReal(),                  fv.getPosition().getY().getDerivative(2).getReal(), 1.0e-10);
518         Assert.assertEquals(fv.getAcceleration().getZ().getReal(),                  fv.getPosition().getZ().getDerivative(2).getReal(), 1.0e-10);
519         Assert.assertEquals(fv.getAcceleration().getX().getReal(),                  fv.getVelocity().getX().getDerivative(1).getReal(), 1.0e-10);
520         Assert.assertEquals(fv.getAcceleration().getY().getReal(),                  fv.getVelocity().getY().getDerivative(1).getReal(), 1.0e-10);
521         Assert.assertEquals(fv.getAcceleration().getZ().getReal(),                  fv.getVelocity().getZ().getDerivative(1).getReal(), 1.0e-10);
522         Assert.assertEquals(fv.getAcceleration().getX().getDerivative(1).getReal(), fv.getVelocity().getX().getDerivative(2).getReal(), 1.0e-10);
523         Assert.assertEquals(fv.getAcceleration().getY().getDerivative(1).getReal(), fv.getVelocity().getY().getDerivative(2).getReal(), 1.0e-10);
524         Assert.assertEquals(fv.getAcceleration().getZ().getDerivative(1).getReal(), fv.getVelocity().getZ().getDerivative(2).getReal(), 1.0e-10);
525 
526         for (double dt = 0; dt < 10; dt += 0.125) {
527             Vector3D p = new PVCoordinates(new Vector3D( 1,  0.1,  10),
528                                            new Vector3D(-1, -0.1, -10),
529                                            new Vector3D(10, -1.0, -100)).shiftedBy(dt).getPosition();
530             Assert.assertEquals(p.getX(), fv.getPosition().getX().taylor(dt).getReal(), 1.0e-14);
531             Assert.assertEquals(p.getY(), fv.getPosition().getY().taylor(dt).getReal(), 1.0e-14);
532             Assert.assertEquals(p.getZ(), fv.getPosition().getZ().taylor(dt).getReal(), 1.0e-14);
533         }
534     }
535 
536     @Test
537     public void testJerkIsVelocitySecondDerivative() {
538         final FieldCartesianOrbit<Decimal64> orbit =
539                         new FieldCartesianOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64(-4947831.), new Decimal64(-3765382.), new Decimal64(-3708221.)),
540                                                                            new FieldVector3D<>(new Decimal64(-2079.), new Decimal64(5291.), new Decimal64(-7842.))),
541                                                         FramesFactory.getEME2000(),
542                                                         FieldAbsoluteDate.getJ2000Epoch(Decimal64Field.getInstance()),
543                                                         new Decimal64(Constants.EIGEN5C_EARTH_MU));
544         FieldPVCoordinates<FieldDerivativeStructure<Decimal64>> fv = orbit.getPVCoordinates().toDerivativeStructurePV(2);
545         FieldVector3D<Decimal64> numericalJerk = differentiate(orbit, o -> o.getPVCoordinates().getAcceleration());
546         Assert.assertEquals(numericalJerk.getX().getReal(),
547                             fv.getVelocity().getX().getPartialDerivative(2).getReal(),
548                             1.0e-13);
549         Assert.assertEquals(numericalJerk.getY().getReal(),
550                             fv.getVelocity().getY().getPartialDerivative(2).getReal(),
551                             1.0e-13);
552         Assert.assertEquals(numericalJerk.getZ().getReal(),
553                             fv.getVelocity().getZ().getPartialDerivative(2).getReal(),
554                             1.0e-13);
555 
556     }
557 
558     @Test
559     public void testJerkIsAccelerationDerivative() {
560         final FieldCartesianOrbit<Decimal64> orbit =
561                         new FieldCartesianOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64(-4947831.), new Decimal64(-3765382.), new Decimal64(-3708221.)),
562                                                                            new FieldVector3D<>(new Decimal64(-2079.), new Decimal64(5291.), new Decimal64(-7842.))),
563                                                         FramesFactory.getEME2000(),
564                                                         FieldAbsoluteDate.getJ2000Epoch(Decimal64Field.getInstance()),
565                                                         new Decimal64(Constants.EIGEN5C_EARTH_MU));
566         FieldPVCoordinates<FieldDerivativeStructure<Decimal64>> fv1 = orbit.getPVCoordinates().toDerivativeStructurePV(1);
567         FieldVector3D<Decimal64> numericalJerk = differentiate(orbit, o -> o.getPVCoordinates().getAcceleration());
568         Assert.assertEquals(numericalJerk.getX().getReal(),
569                             fv1.getAcceleration().getX().getPartialDerivative(1).getReal(),
570                             1.0e-13);
571         Assert.assertEquals(numericalJerk.getY().getReal(),
572                             fv1.getAcceleration().getY().getPartialDerivative(1).getReal(),
573                             1.0e-13);
574         Assert.assertEquals(numericalJerk.getZ().getReal(),
575                             fv1.getAcceleration().getZ().getPartialDerivative(1).getReal(),
576                             1.0e-13);
577 
578         FieldPVCoordinates<FieldDerivativeStructure<Decimal64>> fv2 = orbit.getPVCoordinates().toDerivativeStructurePV(2);
579         Assert.assertEquals(numericalJerk.getX().getReal(),
580                             fv2.getAcceleration().getX().getPartialDerivative(1).getReal(),
581                             1.0e-13);
582         Assert.assertEquals(numericalJerk.getY().getReal(),
583                             fv2.getAcceleration().getY().getPartialDerivative(1).getReal(),
584                             1.0e-13);
585         Assert.assertEquals(numericalJerk.getZ().getReal(),
586                             fv2.getAcceleration().getZ().getPartialDerivative(1).getReal(),
587                             1.0e-13);
588 
589     }
590 
591     @Test
592     public void testJounceIsAccelerationSecondDerivative() {
593         final FieldCartesianOrbit<Decimal64> orbit =
594                         new FieldCartesianOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64(-4947831.), new Decimal64(-3765382.), new Decimal64(-3708221.)),
595                                                                            new FieldVector3D<>(new Decimal64(-2079.), new Decimal64(5291.), new Decimal64(-7842.))),
596                                                         FramesFactory.getEME2000(),
597                                                         FieldAbsoluteDate.getJ2000Epoch(Decimal64Field.getInstance()),
598                                                         new Decimal64(Constants.EIGEN5C_EARTH_MU));
599         FieldPVCoordinates<FieldDerivativeStructure<Decimal64>> fv = orbit.getPVCoordinates().toDerivativeStructurePV(2);
600         FieldVector3D<Decimal64> numericalJounce = differentiate(orbit, o -> {
601         FieldVector3D<FieldDerivativeStructure<Decimal64>> a = o.getPVCoordinates().toDerivativeStructurePV(1).getAcceleration();
602             return new FieldVector3D<>(a.getX().getPartialDerivative(1),
603                                        a.getY().getPartialDerivative(1),
604                                        a.getZ().getPartialDerivative(1));
605         });
606         Assert.assertEquals(numericalJounce.getX().getReal(),
607                             fv.getAcceleration().getX().getPartialDerivative(2).getReal(),
608                             1.0e-15);
609         Assert.assertEquals(numericalJounce.getY().getReal(),
610                             fv.getAcceleration().getY().getPartialDerivative(2).getReal(),
611                             1.0e-15);
612         Assert.assertEquals(numericalJounce.getZ().getReal(),
613                             fv.getAcceleration().getZ().getPartialDerivative(2).getReal(),
614                             1.0e-15);
615 
616     }
617 
618     @Test
619     public void testMomentumDerivative() {
620         final FieldPVCoordinates<Decimal64> pva =
621                         new FieldPVCoordinates<>(new FieldVector3D<>(new Decimal64(-4947831.), new Decimal64(-3765382.), new Decimal64(-3708221.)),
622                                                  new FieldVector3D<>(new Decimal64(-2079.), new Decimal64(5291.), new Decimal64(-7842.)));
623         final FieldVector3D<Decimal64> p  = pva.getPosition();
624         final FieldVector3D<Decimal64>  v  = pva.getVelocity();
625         final FieldVector3D<Decimal64>  a  = pva.getAcceleration();
626         final Decimal64   r2 = p.getNormSq();
627         final Decimal64   r  = r2.sqrt();
628         final FieldVector3D<Decimal64>  keplerianJerk = new FieldVector3D<>(FieldVector3D.dotProduct(p, v).multiply(-2).divide(r2), a,
629                                                                             a.getNorm().negate().divide(r), v);
630         final FieldPVCoordinates<Decimal64> velocity = new FieldPVCoordinates<>(v, a, keplerianJerk);
631         final FieldVector3D<Decimal64>  momentumRef    = pva.getMomentum();
632         final FieldVector3D<Decimal64>  momentumDotRef = pva.crossProduct(velocity).getVelocity();
633 
634         final FieldVector3D<FieldDerivativeStructure<Decimal64>> momentumDot = pva.toDerivativeStructurePV(1).getMomentum();
635         Assert.assertEquals(momentumRef.getX().getReal(),    momentumDot.getX().getReal(),                         1.0e-15);
636         Assert.assertEquals(momentumRef.getY().getReal(),    momentumDot.getY().getReal(),                         1.0e-15);
637         Assert.assertEquals(momentumRef.getZ().getReal(),    momentumDot.getZ().getReal(),                         1.0e-15);
638         Assert.assertEquals(momentumDotRef.getX().getReal(), momentumDot.getX().getPartialDerivative(1).getReal(), 1.0e-15);
639         Assert.assertEquals(momentumDotRef.getY().getReal(), momentumDot.getY().getPartialDerivative(1).getReal(), 1.0e-15);
640         Assert.assertEquals(momentumDotRef.getZ().getReal(), momentumDot.getZ().getPartialDerivative(1).getReal(), 1.0e-15);
641 
642     }
643 
644     @Test
645     public void testShift() {
646         FieldVector3D<DerivativeStructure> p1 = createVector(1, 0.1, 10, 6);
647         FieldVector3D<DerivativeStructure> p2 = createVector(2, 0.2, 20, 6);
648         FieldVector3D<DerivativeStructure> v  = createVector(-1, -0.1, -10, 6);
649         checkPV(new FieldPVCoordinates<>(p2, v), new FieldPVCoordinates<>(p1, v).shiftedBy(-1.0), 1.0e-15);
650         Assert.assertEquals(0.0,
651                             FieldPVCoordinates.estimateVelocity(p1, p2, -1.0).subtract(v).getNorm().getValue(),
652                             1.0e-15);
653     }
654 
655     @Test
656     public void testGetters() {
657         FieldVector3D<DerivativeStructure> p = createVector(1, 0.1, 10, 6);
658         FieldVector3D<DerivativeStructure> v = createVector(-0.1, 1, 0, 6);
659         FieldPVCoordinates<DerivativeStructure> pv = new FieldPVCoordinates<>(p, v);
660         Assert.assertEquals(0, FieldVector3D.distance(p, pv.getPosition()).getValue(), 1.0e-15);
661         Assert.assertEquals(0, FieldVector3D.distance(v, pv.getVelocity()).getValue(), 1.0e-15);
662         Assert.assertEquals(0, FieldVector3D.distance(createVector(-10, -1, 1.01, 6), pv.getMomentum()).getValue(), 1.0e-15);
663 
664         FieldPVCoordinates<DerivativeStructure> pvn = pv.negate();
665         Assert.assertEquals(0, FieldVector3D.distance(createVector(-1, -0.1, -10, 6), pvn.getPosition()).getValue(), 1.0e-15);
666         Assert.assertEquals(0, FieldVector3D.distance(createVector(0.1, -1, 0, 6), pvn.getVelocity()).getValue(), 1.0e-15);
667         Assert.assertEquals(0, FieldVector3D.distance(createVector(-10, -1, 1.01, 6), pvn.getMomentum()).getValue(), 1.0e-15);
668     }
669 
670     @Test
671     public void testToString() {
672         FieldPVCoordinates<DerivativeStructure> pv = new FieldPVCoordinates<>(createVector( 1,  0.1,  10, 6),
673                                                                               createVector(-1, -0.1, -10, 6),
674                                                                               createVector(10,  1.0, 100, 6));
675         Assert.assertEquals("{P(1.0, 0.1, 10.0), V(-1.0, -0.1, -10.0), A(10.0, 1.0, 100.0)}", pv.toString());
676     }
677 
678     @Test
679     public void testNormalize() {
680         DSFactory factory = new DSFactory(1, 2);
681         RandomGenerator generator = new Well19937a(0x7ede9376e4e1ab5al);
682         FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(5, 1.0e-3);
683         for (int i = 0; i < 200; ++i) {
684             final FieldPVCoordinates<DerivativeStructure> pv = randomPVCoordinates(generator, 1e6, 1e3, 1.0);
685             DerivativeStructure x =
686                     differentiator.differentiate(new UnivariateFunction() {
687                         public double value(double t) {
688                             return pv.shiftedBy(t).getPosition().normalize().getX().getValue();
689                         }
690                     }).value(factory.variable(0, 0.0));
691             DerivativeStructure y =
692                     differentiator.differentiate(new UnivariateFunction() {
693                         public double value(double t) {
694                             return pv.shiftedBy(t).getPosition().normalize().getY().getValue();
695                         }
696                     }).value(factory.variable(0, 0.0));
697             DerivativeStructure z =
698                     differentiator.differentiate(new UnivariateFunction() {
699                         public double value(double t) {
700                             return pv.shiftedBy(t).getPosition().normalize().getZ().getValue();
701                         }
702                     }).value(factory.variable(0, 0.0));
703             FieldPVCoordinates<DerivativeStructure> normalized = pv.normalize();
704             Assert.assertEquals(x.getValue(),              normalized.getPosition().getX().getValue(),     1.0e-16);
705             Assert.assertEquals(y.getValue(),              normalized.getPosition().getY().getValue(),     1.0e-16);
706             Assert.assertEquals(z.getValue(),              normalized.getPosition().getZ().getValue(),     1.0e-16);
707             Assert.assertEquals(x.getPartialDerivative(1), normalized.getVelocity().getX().getValue(),     3.0e-13);
708             Assert.assertEquals(y.getPartialDerivative(1), normalized.getVelocity().getY().getValue(),     3.0e-13);
709             Assert.assertEquals(z.getPartialDerivative(1), normalized.getVelocity().getZ().getValue(),     3.0e-13);
710             Assert.assertEquals(x.getPartialDerivative(2), normalized.getAcceleration().getX().getValue(), 6.0e-10);
711             Assert.assertEquals(y.getPartialDerivative(2), normalized.getAcceleration().getY().getValue(), 6.0e-10);
712             Assert.assertEquals(z.getPartialDerivative(2), normalized.getAcceleration().getZ().getValue(), 6.0e-10);
713         }
714     }
715 
716     private FieldVector3D<DerivativeStructure> randomVector(RandomGenerator random, double norm) {
717         double n = random.nextDouble() * norm;
718         double x = random.nextDouble();
719         double y = random.nextDouble();
720         double z = random.nextDouble();
721         n = n / FastMath.sqrt(x * x + y * y + z * z);
722         return createVector(n * x, n * y, n * z, 3);
723     }
724 
725     private FieldPVCoordinates<DerivativeStructure> randomPVCoordinates(RandomGenerator random,
726                                                                         double norm0, double norm1, double norm2) {
727         FieldVector3D<DerivativeStructure> p0 = randomVector(random, norm0);
728         FieldVector3D<DerivativeStructure> p1 = randomVector(random, norm1);
729         FieldVector3D<DerivativeStructure> p2 = randomVector(random, norm2);
730         return new FieldPVCoordinates<>(p0, p1, p2);
731     }
732 
733     private FieldVector3D<DerivativeStructure> createVector(double x, double y, double z, int params) {
734         DSFactory factory = new DSFactory(params, 1);
735         return new FieldVector3D<>(factory.variable(0, x),
736                                    factory.variable(1, y),
737                                    factory.variable(2, z));
738     }
739 
740     private <T extends CalculusFieldElement<T>> void checkPV(FieldPVCoordinates<T> expected, FieldPVCoordinates<T> real, double epsilon) {
741         Assert.assertEquals(expected.getPosition().getX().getReal(), real.getPosition().getX().getReal(), epsilon);
742         Assert.assertEquals(expected.getPosition().getY().getReal(), real.getPosition().getY().getReal(), epsilon);
743         Assert.assertEquals(expected.getPosition().getZ().getReal(), real.getPosition().getZ().getReal(), epsilon);
744         Assert.assertEquals(expected.getVelocity().getX().getReal(), real.getVelocity().getX().getReal(), epsilon);
745         Assert.assertEquals(expected.getVelocity().getY().getReal(), real.getVelocity().getY().getReal(), epsilon);
746         Assert.assertEquals(expected.getVelocity().getZ().getReal(), real.getVelocity().getZ().getReal(), epsilon);
747     }
748 
749     private interface OrbitFunction<T extends CalculusFieldElement<T>>  {
750         FieldVector3D<T> apply(final FieldCartesianOrbit<T> o);
751     }
752 
753     private <T extends CalculusFieldElement<T>> FieldVector3D<T> differentiate(FieldCartesianOrbit<T> orbit,
754                                                                            OrbitFunction<T> picker) {
755         try {
756             FieldHermiteInterpolator<T> interpolator = new FieldHermiteInterpolator<>();
757             final T step = orbit.getDate().getField().getZero().add(0.01); 
758             for (int i = -4; i < 4; ++i) {
759                 T dt = step.multiply(i);
760                 interpolator.addSamplePoint(dt, picker.apply(orbit.shiftedBy(dt)).toArray());
761             }
762             return new FieldVector3D<>(interpolator.derivatives(orbit.getDate().getField().getZero(), 1)[1]);
763         } catch (OrekitException oe) {
764             return null;
765         }
766      }
767 
768 }