1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.bodies;
18
19 import java.util.ArrayList;
20 import java.util.List;
21 import java.util.stream.DoubleStream;
22 import java.util.stream.Stream;
23
24 import org.hipparchus.CalculusFieldElement;
25 import org.hipparchus.Field;
26 import org.hipparchus.analysis.UnivariateFunction;
27 import org.hipparchus.analysis.differentiation.DSFactory;
28 import org.hipparchus.analysis.differentiation.DerivativeStructure;
29 import org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator;
30 import org.hipparchus.analysis.differentiation.UnivariateDifferentiableFunction;
31 import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
32 import org.hipparchus.exception.LocalizedCoreFormats;
33 import org.hipparchus.geometry.euclidean.oned.Vector1D;
34 import org.hipparchus.geometry.euclidean.threed.FieldLine;
35 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
36 import org.hipparchus.geometry.euclidean.threed.Line;
37 import org.hipparchus.geometry.euclidean.threed.Vector3D;
38 import org.hipparchus.geometry.euclidean.twod.Vector2D;
39 import org.hipparchus.random.SobolSequenceGenerator;
40 import org.hipparchus.util.Binary64;
41 import org.hipparchus.util.Binary64Field;
42 import org.hipparchus.util.FastMath;
43 import org.hipparchus.util.MathUtils;
44 import org.junit.jupiter.api.Assertions;
45 import org.junit.jupiter.api.BeforeEach;
46 import org.junit.jupiter.api.Test;
47 import org.orekit.Utils;
48 import org.orekit.errors.OrekitException;
49 import org.orekit.errors.OrekitMessages;
50 import org.orekit.frames.FieldStaticTransform;
51 import org.orekit.frames.Frame;
52 import org.orekit.frames.FramesFactory;
53 import org.orekit.frames.StaticTransform;
54 import org.orekit.models.earth.ReferenceEllipsoid;
55 import org.orekit.orbits.CircularOrbit;
56 import org.orekit.orbits.EquinoctialOrbit;
57 import org.orekit.orbits.Orbit;
58 import org.orekit.orbits.PositionAngleType;
59 import org.orekit.time.AbsoluteDate;
60 import org.orekit.time.DateComponents;
61 import org.orekit.time.FieldAbsoluteDate;
62 import org.orekit.time.TimeComponents;
63 import org.orekit.time.TimeInterpolator;
64 import org.orekit.time.TimeScalesFactory;
65 import org.orekit.utils.CartesianDerivativesFilter;
66 import org.orekit.utils.Constants;
67 import org.orekit.utils.IERSConventions;
68 import org.orekit.utils.PVCoordinates;
69 import org.orekit.utils.PVCoordinatesProvider;
70 import org.orekit.utils.TimeStampedPVCoordinates;
71 import org.orekit.utils.TimeStampedPVCoordinatesHermiteInterpolator;
72
73 import static org.hamcrest.MatcherAssert.assertThat;
74 import static org.orekit.OrekitMatchers.closeTo;
75
76
77 class OneAxisEllipsoidTest {
78
79 @Test
80 void testStandard() {
81 checkCartesianToEllipsoidic(6378137.0, 1.0 / 298.257222101,
82 4637885.347, 121344.608, 4362452.869,
83 0.026157811533131, 0.757987116290729, 260.455572965555);
84 }
85
86 @Test
87 void testLongitudeZero() {
88 checkCartesianToEllipsoidic(6378137.0, 1.0 / 298.257222101,
89 6378400.0, 0, 6379000.0,
90 0.0, 0.787815771252351, 2653416.77864152);
91 }
92
93 @Test
94 void testLongitudePi() {
95 checkCartesianToEllipsoidic(6378137.0, 1.0 / 298.257222101,
96 -6379999.0, 0, 6379000.0,
97 3.14159265358979, 0.787690146758403, 2654544.7767725);
98 }
99
100 @Test
101 void testNorthPole() {
102 checkCartesianToEllipsoidic(6378137.0, 1.0 / 298.257222101,
103 0.0, 0.0, 7000000.0,
104 0.0, 1.57079632679490, 643247.685859644);
105 }
106
107 @Test
108 void testEquator() {
109 checkCartesianToEllipsoidic(6378137.0, 1.0 / 298.257222101,
110 6379888.0, 6377000.0, 0.0,
111 0.785171775899913, 0.0, 2642345.24279301);
112 }
113
114 @Test
115 void testNoFlattening() {
116 final double r = 7000000.0;
117 final double lambda = 2.345;
118 final double phi = -1.23;
119 final double cL = FastMath.cos(lambda);
120 final double sL = FastMath.sin(lambda);
121 final double cH = FastMath.cos(phi);
122 final double sH = FastMath.sin(phi);
123 checkCartesianToEllipsoidic(6378137.0, 0,
124 r * cL * cH, r * sL * cH, r * sH,
125 lambda, phi, r - 6378137.0);
126 }
127
128 @Test
129 void testNoFlatteningPolar() {
130 final double r = 1000.0;
131 final double h = 100;
132 checkCartesianToEllipsoidic(r, 0,
133 0, 0, r + h,
134 0, 0.5 * FastMath.PI, h);
135 checkCartesianToEllipsoidic(r, 0,
136 0, 0, r - h,
137 0, 0.5 * FastMath.PI, -h);
138 checkCartesianToEllipsoidic(r, 0,
139 0, 0, -(r + h),
140 0, -0.5 * FastMath.PI, h);
141 checkCartesianToEllipsoidic(r, 0,
142 0, 0, -(r - h),
143 0, -0.5 * FastMath.PI, -h);
144 }
145
146 @Test
147 void testOnSurface() {
148 Vector3D surfacePoint = new Vector3D(-1092200.775949484,
149 -3944945.7282234835,
150 4874931.946956173);
151 OneAxisEllipsoid earthShape = new OneAxisEllipsoid(6378136.460, 1 / 298.257222101,
152 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
153 GeodeticPoint gp = earthShape.transform(surfacePoint, earthShape.getBodyFrame(),
154 AbsoluteDate.J2000_EPOCH);
155 Vector3D rebuilt = earthShape.transform(gp);
156 Assertions.assertEquals(0, rebuilt.distance(surfacePoint), 3.0e-9);
157 }
158
159 @Test
160 void testInside3Roots() {
161 checkCartesianToEllipsoidic(6378137.0, 1.0 / 298.257,
162 9219.0, -5322.0, 6056743.0,
163 5.75963470503781, 1.56905114598949, -300000.009586231);
164 }
165
166 @Test
167 void testInsideLessThan3Roots() {
168 checkCartesianToEllipsoidic(6378137.0, 1.0 / 298.257,
169 1366863.0, -789159.0, -5848.988,
170 -0.523598928689, -0.00380885831963, -4799808.27951);
171 }
172
173 @Test
174 void testOutside() {
175 checkCartesianToEllipsoidic(6378137.0, 1.0 / 298.257,
176 5722966.0, -3304156.0, -24621187.0,
177 5.75958652642615, -1.3089969725151, 19134410.3342696);
178 }
179
180 @Test
181 void testGeoCar() {
182 OneAxisEllipsoid model =
183 new OneAxisEllipsoid(6378137.0, 1.0 / 298.257222101,
184 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
185 GeodeticPoint nsp =
186 new GeodeticPoint(0.852479154923577, 0.0423149994747243, 111.6);
187 Vector3D p = model.transform(nsp);
188 Assertions.assertEquals(4201866.69291890, p.getX(), 1.0e-6);
189 Assertions.assertEquals(177908.184625686, p.getY(), 1.0e-6);
190 Assertions.assertEquals(4779203.64408617, p.getZ(), 1.0e-6);
191 }
192
193 @Test
194 void testGroundProjectionPosition() {
195 OneAxisEllipsoid model =
196 new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
197 Constants.WGS84_EARTH_FLATTENING,
198 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
199
200 TimeStampedPVCoordinates initPV =
201 new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH.shiftedBy(584.),
202 new Vector3D(3220103., 69623., 6449822.),
203 new Vector3D(6414.7, -2006., -3180.),
204 Vector3D.ZERO);
205 Frame eme2000 = FramesFactory.getEME2000();
206 Orbit orbit = new EquinoctialOrbit(initPV, eme2000, Constants.EIGEN5C_EARTH_MU);
207
208 for (double dt = 0; dt < 3600.0; dt += 60.0) {
209
210 AbsoluteDate date = orbit.getDate().shiftedBy(dt);
211 Vector3D pos = orbit.getPosition(date, eme2000);
212 Vector3D groundPV = model.projectToGround(pos, date, eme2000);
213 Vector3D groundP = model.projectToGround(pos, date, eme2000);
214
215
216 Assertions.assertEquals(model.transform(pos, eme2000, date).getLatitude(),
217 model.transform(groundPV, eme2000, date).getLatitude(),
218 1.0e-10);
219 Assertions.assertEquals(model.transform(pos, eme2000, date).getLongitude(),
220 model.transform(groundPV, eme2000, date).getLongitude(),
221 1.0e-10);
222 Assertions.assertEquals(0.0, Vector3D.distance(groundP, groundPV), 1.0e-15 * groundP.getNorm());
223
224 }
225
226 }
227
228 @Test
229 void testGroundProjectionDerivatives() {
230 Frame itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
231 Frame eme2000 = FramesFactory.getEME2000();
232 OneAxisEllipsoid model =
233 new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
234 Constants.WGS84_EARTH_FLATTENING,
235 itrf);
236
237 TimeStampedPVCoordinates initPV =
238 new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH.shiftedBy(584.),
239 new Vector3D(3220103., 69623., 6449822.),
240 new Vector3D(6414.7, -2006., -3180.),
241 Vector3D.ZERO);
242 Orbit orbit = new EquinoctialOrbit(initPV, eme2000, Constants.EIGEN5C_EARTH_MU);
243
244 double[] errors = derivativesErrors(orbit, orbit.getDate(), eme2000, model);
245 Assertions.assertEquals(0, errors[0], 1.0e-16);
246 Assertions.assertEquals(0, errors[1], 2.0e-12);
247 Assertions.assertEquals(0, errors[2], 2.0e-4);
248
249 }
250
251 @Test
252 void testGroundToGroundIssue181() {
253 Frame itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
254 Frame eme2000 = FramesFactory.getEME2000();
255 OneAxisEllipsoid model =
256 new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
257 Constants.WGS84_EARTH_FLATTENING,
258 itrf);
259
260 TimeStampedPVCoordinates initPV =
261 new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH.shiftedBy(584.),
262 new Vector3D(3220103., 69623., 6449822.),
263 new Vector3D(6414.7, -2006., -3180.),
264 Vector3D.ZERO);
265 TimeStampedPVCoordinates body = itrf.getTransformTo(eme2000, initPV.getDate()).transformPVCoordinates(initPV);
266 TimeStampedPVCoordinates ground1 = model.projectToGround(body, itrf);
267 TimeStampedPVCoordinates ground2 = model.projectToGround(ground1, itrf);
268 Assertions.assertEquals(0.0, Vector3D.distance(ground1.getPosition(), ground2.getPosition()), 1.0e-12);
269 Assertions.assertEquals(0.0, Vector3D.distance(ground1.getVelocity(), ground2.getVelocity()), 2.0e-12);
270 Assertions.assertEquals(0.0, Vector3D.distance(ground1.getAcceleration(), ground2.getAcceleration()), 1.0e-12);
271
272 }
273
274 private double[] derivativesErrors(PVCoordinatesProvider provider, AbsoluteDate date, Frame frame,
275 OneAxisEllipsoid model) {
276 List<TimeStampedPVCoordinates> pvList = new ArrayList<>();
277 List<TimeStampedPVCoordinates> groundPVList = new ArrayList<>();
278 for (double dt = -0.25; dt <= 0.25; dt += 0.125) {
279 TimeStampedPVCoordinates shiftedPV = provider.getPVCoordinates(date.shiftedBy(dt), frame);
280 Vector3D p = model.projectToGround(shiftedPV.getPosition(), shiftedPV.getDate(), frame);
281 pvList.add(shiftedPV);
282 groundPVList.add(new TimeStampedPVCoordinates(shiftedPV.getDate(),
283 p, Vector3D.ZERO, Vector3D.ZERO));
284 }
285
286
287 final TimeInterpolator<TimeStampedPVCoordinates> interpolator =
288 new TimeStampedPVCoordinatesHermiteInterpolator(pvList.size(), CartesianDerivativesFilter.USE_P);
289
290 final TimeInterpolator<TimeStampedPVCoordinates> interpolatorGround =
291 new TimeStampedPVCoordinatesHermiteInterpolator(groundPVList.size(), CartesianDerivativesFilter.USE_P);
292
293 TimeStampedPVCoordinates computed = model.projectToGround(interpolator.interpolate(date, pvList), frame);
294 TimeStampedPVCoordinates reference = interpolatorGround.interpolate(date, groundPVList);
295
296 TimeStampedPVCoordinates pv0 = provider.getPVCoordinates(date, frame);
297 Vector3D p0 = pv0.getPosition();
298 Vector3D v0 = pv0.getVelocity();
299 Vector3D a0 = pv0.getAcceleration();
300
301 return new double[] {
302 Vector3D.distance(computed.getPosition(), reference.getPosition()) / p0.getNorm(),
303 Vector3D.distance(computed.getVelocity(), reference.getVelocity()) / v0.getNorm(),
304 Vector3D.distance(computed.getAcceleration(), reference.getAcceleration()) / a0.getNorm(),
305 };
306
307 }
308
309 @Test
310 void testGroundProjectionTaylor() {
311 Frame itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
312 Frame eme2000 = FramesFactory.getEME2000();
313 OneAxisEllipsoid model =
314 new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
315 Constants.WGS84_EARTH_FLATTENING,
316 itrf);
317
318 TimeStampedPVCoordinates initPV =
319 new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH.shiftedBy(584.),
320 new Vector3D(3220103., 69623., 6449822.),
321 new Vector3D(6414.7, -2006., -3180.),
322 Vector3D.ZERO);
323 Orbit orbit = new EquinoctialOrbit(initPV, eme2000, Constants.EIGEN5C_EARTH_MU);
324
325 TimeStampedPVCoordinates pv0 = orbit.getPVCoordinates(orbit.getDate(), model.getBodyFrame());
326 PVCoordinatesProvider groundTaylor =
327 model.projectToGround(pv0, model.getBodyFrame()).toTaylorProvider(model.getBodyFrame());
328
329 TimeStampedPVCoordinates g0 = groundTaylor.getPVCoordinates(orbit.getDate(), model.getBodyFrame());
330 Vector3D zenith = pv0.getPosition().subtract(g0.getPosition()).normalize();
331 Vector3D acrossTrack = Vector3D.crossProduct(zenith, g0.getVelocity()).normalize();
332 Vector3D alongTrack = Vector3D.crossProduct(acrossTrack, zenith).normalize();
333 for (double dt = -1; dt < 1; dt += 0.01) {
334 AbsoluteDate date = orbit.getDate().shiftedBy(dt);
335 Vector3D taylorP = groundTaylor.getPosition(date, model.getBodyFrame());
336 Vector3D refP = model.projectToGround(orbit.getPosition(date, model.getBodyFrame()),
337 date, model.getBodyFrame());
338 Vector3D delta = taylorP.subtract(refP);
339 Assertions.assertEquals(0.0, Vector3D.dotProduct(delta, alongTrack), 0.0015);
340 Assertions.assertEquals(0.0, Vector3D.dotProduct(delta, acrossTrack), 0.0007);
341 Assertions.assertEquals(0.0, Vector3D.dotProduct(delta, zenith), 0.00002);
342 }
343
344 }
345
346 @Test
347 void testLineIntersection() {
348 AbsoluteDate date = AbsoluteDate.J2000_EPOCH;
349 Frame frame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
350
351 OneAxisEllipsoid model = new OneAxisEllipsoid(100.0, 0.9, frame);
352 Vector3D point = new Vector3D(0.0, 93.7139699, 3.5930796);
353 Vector3D direction = new Vector3D(0.0, 1.0, 1.0);
354 Line line = new Line(point, point.add(direction), 1.0e-10);
355 GeodeticPoint gp = model.getIntersectionPoint(line, point, frame, date);
356 Assertions.assertEquals(0.0, gp.getAltitude(), 1.0e-12);
357 Assertions.assertTrue(line.contains(model.transform(gp)));
358
359 model = new OneAxisEllipsoid(100.0, 0.9, frame);
360 point = new Vector3D(0.0, -93.7139699, -3.5930796);
361 direction = new Vector3D(0.0, -1.0, -1.0);
362 line = new Line(point, point.add(direction), 1.0e-10).revert();
363 gp = model.getIntersectionPoint(line, point, frame, date);
364 Assertions.assertTrue(line.contains(model.transform(gp)));
365
366 model = new OneAxisEllipsoid(100.0, 0.9, frame);
367 point = new Vector3D(0.0, -93.7139699, 3.5930796);
368 direction = new Vector3D(0.0, -1.0, 1.0);
369 line = new Line(point, point.add(direction), 1.0e-10);
370 gp = model.getIntersectionPoint(line, point, frame, date);
371 Assertions.assertTrue(line.contains(model.transform(gp)));
372
373 model = new OneAxisEllipsoid(100.0, 0.9, frame);
374 point = new Vector3D(-93.7139699, 0.0, 3.5930796);
375 direction = new Vector3D(-1.0, 0.0, 1.0);
376 line = new Line(point, point.add(direction), 1.0e-10);
377 gp = model.getIntersectionPoint(line, point, frame, date);
378 Assertions.assertTrue(line.contains(model.transform(gp)));
379 Assertions.assertFalse(line.contains(new Vector3D(0, 0, 7000000)));
380
381 point = new Vector3D(0.0, 0.0, 110);
382 direction = new Vector3D(0.0, 0.0, 1.0);
383 line = new Line(point, point.add(direction), 1.0e-10);
384 gp = model.getIntersectionPoint(line, point, frame, date);
385 Assertions.assertEquals(FastMath.PI/2, gp.getLatitude(), 1.0e-12);
386
387 point = new Vector3D(0.0, 110, 0);
388 direction = new Vector3D(0.0, 1.0, 0.0);
389 line = new Line(point, point.add(direction), 1.0e-10);
390 gp = model.getIntersectionPoint(line, point, frame, date);
391 Assertions.assertEquals(0, gp.getLatitude(), 1.0e-12);
392
393 }
394
395 @Test
396 void testNoLineIntersection() {
397 AbsoluteDate date = AbsoluteDate.J2000_EPOCH;
398 Frame frame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
399 OneAxisEllipsoid model = new OneAxisEllipsoid(100.0, 0.9, frame);
400 Vector3D point = new Vector3D(0.0, 93.7139699, 3.5930796);
401 Vector3D direction = new Vector3D(0.0, 9.0, -2.0);
402 Line line = new Line(point, point.add(direction), 1.0e-10);
403 Assertions.assertNull(model.getIntersectionPoint(line, point, frame, date));
404 }
405
406 @Test
407 void testNegativeZ() {
408 AbsoluteDate date = AbsoluteDate.J2000_EPOCH;
409 Frame frame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
410 OneAxisEllipsoid model = new OneAxisEllipsoid(90.0, 5.0 / 9.0, frame);
411 Vector3D point = new Vector3D(140.0, 0.0, -30.0);
412 GeodeticPoint gp = model.transform(point, frame, date);
413 Vector3D rebuilt = model.transform(gp);
414 Assertions.assertEquals(0.0, rebuilt.distance(point), 1.0e-10);
415 }
416
417 @Test
418 void testNumerousIteration() {
419
420
421
422
423 AbsoluteDate date = AbsoluteDate.J2000_EPOCH;
424 Frame frame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
425 OneAxisEllipsoid model = new OneAxisEllipsoid(100.0, 999.0 / 1000.0, frame);
426 Vector3D point = new Vector3D(100.001, 0.0, 1.0);
427 GeodeticPoint gp = model.transform(point, frame, date);
428 Vector3D rebuilt = model.transform(gp);
429 Assertions.assertEquals(0.0, rebuilt.distance(point), 1.2e-11);
430 }
431
432 @Test
433 void testEquatorialInside() {
434 AbsoluteDate date = AbsoluteDate.J2000_EPOCH;
435 Frame frame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
436 OneAxisEllipsoid model = new OneAxisEllipsoid(90.0, 5.0 / 9.0, frame);
437 for (double rho = 0; rho < model.getEquatorialRadius(); rho += 0.01) {
438 Vector3D point = new Vector3D(rho, 0.0, 0.0);
439 GeodeticPoint gp = model.transform(point, frame, date);
440 Vector3D rebuilt = model.transform(gp);
441 Assertions.assertEquals(0.0, rebuilt.distance(point), 1.0e-10);
442 }
443 }
444
445 @Test
446 void testFarPoint() {
447 AbsoluteDate date = AbsoluteDate.J2000_EPOCH;
448 Frame frame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
449 OneAxisEllipsoid model = new OneAxisEllipsoid(90.0, 5.0 / 9.0, frame);
450 Vector3D point = new Vector3D(1.0e15, 2.0e15, -1.0e12);
451 GeodeticPoint gp = model.transform(point, frame, date);
452 Vector3D rebuilt = model.transform(gp);
453 Assertions.assertEquals(0.0, rebuilt.distance(point), 1.0e-15 * point.getNorm());
454 }
455
456 @Test
457 void testIssue141() {
458 AbsoluteDate date = new AbsoluteDate("2002-03-06T20:50:20.44188731559965033", TimeScalesFactory.getUTC());
459 Frame frame = FramesFactory.getGTOD(IERSConventions.IERS_1996, true);
460 OneAxisEllipsoid model = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
461 Constants.WGS84_EARTH_FLATTENING,
462 frame);
463 Vector3D point = new Vector3D(-6838696.282102453, -2148321.403361013, -0.011907944179711194);
464 GeodeticPoint gp = model.transform(point, frame, date);
465 Vector3D rebuilt = model.transform(gp);
466 Assertions.assertEquals(0.0, rebuilt.distance(point), 1.0e-15 * point.getNorm());
467 }
468
469 @Test
470 void testIntersectionFromPoints() {
471 AbsoluteDate date = new AbsoluteDate(new DateComponents(2008, 3, 21),
472 TimeComponents.H12,
473 TimeScalesFactory.getUTC());
474
475 Frame frame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
476 OneAxisEllipsoid earth = new OneAxisEllipsoid(6378136.460, 1 / 298.257222101, frame);
477
478
479
480 final double mu = 3.9860047e14;
481 CircularOrbit circ =
482 new CircularOrbit(7178000.0, 0.5e-4, 0., FastMath.toRadians(90.), FastMath.toRadians(60.),
483 FastMath.toRadians(90.), PositionAngleType.MEAN,
484 FramesFactory.getEME2000(), date, mu);
485
486
487 PVCoordinates pvSatEME2000 = circ.getPVCoordinates();
488 Vector3D pSatItrf = frame.getStaticTransformTo(FramesFactory.getEME2000(), date).transformPosition(pvSatEME2000.getPosition());
489
490
491 GeodeticPoint geoPoint = new GeodeticPoint(FastMath.toRadians(70.), FastMath.toRadians(60.), 0.);
492 Vector3D pointItrf = earth.transform(geoPoint);
493 Line line = new Line(pSatItrf, pointItrf, 1.0e-10);
494 GeodeticPoint geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
495 Assertions.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle);
496 Assertions.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle);
497
498
499 geoPoint = new GeodeticPoint(FastMath.toRadians(65.), FastMath.toRadians(-120.), 0.);
500 pointItrf = earth.transform(geoPoint);
501 line = new Line(pSatItrf, pointItrf, 1.0e-10);
502 geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
503 Assertions.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle);
504 Assertions.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle);
505
506
507 geoPoint = new GeodeticPoint(FastMath.toRadians(30.), FastMath.toRadians(60.), 0.);
508 pointItrf = earth.transform(geoPoint);
509 line = new Line(pSatItrf, pointItrf, 1.0e-10);
510
511 geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
512
513
514 Assertions.assertEquals(1.04437199, geoInter.getLongitude(), Utils.epsilonAngle);
515 Assertions.assertEquals(1.36198012, geoInter.getLatitude(), Utils.epsilonAngle);
516
517
518
519 circ =
520 new CircularOrbit(7178000.0, 0.5e-4, 0., FastMath.toRadians(1.e-4), FastMath.toRadians(0.),
521 FastMath.toRadians(0.), PositionAngleType.MEAN,
522 FramesFactory.getEME2000(), date, mu);
523
524
525 pvSatEME2000 = circ.getPVCoordinates();
526 pSatItrf = frame.getStaticTransformTo(FramesFactory.getEME2000(), date).transformPosition(pvSatEME2000.getPosition());
527
528
529 geoPoint = new GeodeticPoint(FastMath.toRadians(5.), FastMath.toRadians(0.), 0.);
530 pointItrf = earth.transform(geoPoint);
531 line = new Line(pSatItrf, pointItrf, 1.0e-10);
532 Assertions.assertTrue(line.toSubSpace(pSatItrf).getX() < 0);
533 geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
534 Assertions.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle);
535 Assertions.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle);
536
537
538 GeodeticPoint geoInter2 = earth.getIntersectionPoint(line, line.toSpace(new Vector1D(-line.toSubSpace(pSatItrf).getX())), frame, date);
539 Assertions.assertTrue(FastMath.abs(geoInter.getLongitude() - geoInter2.getLongitude()) > FastMath.toRadians(0.1));
540 Assertions.assertTrue(FastMath.abs(geoInter.getLatitude() - geoInter2.getLatitude()) > FastMath.toRadians(0.1));
541
542
543 geoPoint = new GeodeticPoint(FastMath.toRadians(-5.), FastMath.toRadians(0.), 0.);
544 pointItrf = earth.transform(geoPoint);
545 line = new Line(pSatItrf, pointItrf, 1.0e-10);
546 geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
547 Assertions.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle);
548 Assertions.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle);
549
550
551 geoPoint = new GeodeticPoint(FastMath.toRadians(40.), FastMath.toRadians(0.), 0.);
552 pointItrf = earth.transform(geoPoint);
553 line = new Line(pSatItrf, pointItrf, 1.0e-10);
554 geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
555 Assertions.assertEquals(-0.00768481, geoInter.getLongitude(), Utils.epsilonAngle);
556 Assertions.assertEquals( 0.32180410, geoInter.getLatitude(), Utils.epsilonAngle);
557
558
559
560
561 circ =
562 new CircularOrbit(7178000.0, 0.5e-4, 0., FastMath.toRadians(50.), FastMath.toRadians(0.),
563 FastMath.toRadians(90.), PositionAngleType.MEAN,
564 FramesFactory.getEME2000(), date, mu);
565
566
567 pvSatEME2000 = circ.getPVCoordinates();
568 pSatItrf = frame.getStaticTransformTo(FramesFactory.getEME2000(), date).transformPosition(pvSatEME2000.getPosition());
569
570
571 geoPoint = new GeodeticPoint(FastMath.toRadians(40.), FastMath.toRadians(90.), 0.);
572 pointItrf = earth.transform(geoPoint);
573 line = new Line(pSatItrf, pointItrf, 1.0e-10);
574 geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
575 Assertions.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle);
576 Assertions.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle);
577
578
579 geoPoint = new GeodeticPoint(FastMath.toRadians(60.), FastMath.toRadians(90.), 0.);
580 pointItrf = earth.transform(geoPoint);
581 line = new Line(pSatItrf, pointItrf, 1.0e-10);
582 geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
583 Assertions.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle);
584 Assertions.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle);
585
586
587 geoPoint = new GeodeticPoint(FastMath.toRadians(0.), FastMath.toRadians(90.), 0.);
588 pointItrf = earth.transform(geoPoint);
589 line = new Line(pSatItrf, pointItrf, 1.0e-10);
590 geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
591 Assertions.assertEquals(FastMath.toRadians(89.5364061088196), geoInter.getLongitude(), Utils.epsilonAngle);
592 Assertions.assertEquals(FastMath.toRadians(35.555543683351125), geoInter.getLatitude(), Utils.epsilonAngle);
593
594 }
595
596 @Test
597 void testMovingGeodeticPointSymmetry() {
598
599 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
600 Constants.WGS84_EARTH_FLATTENING,
601 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
602 double lat0 = FastMath.toRadians(60.0);
603 double lon0 = FastMath.toRadians(25.0);
604 double alt0 = 100.0;
605 double lat1 = 1.0e-3;
606 double lon1 = -2.0e-3;
607 double alt1 = 1.2;
608 double lat2 = -1.0e-5;
609 double lon2 = -3.0e-5;
610 double alt2 = -0.01;
611 final DSFactory factory = new DSFactory(1, 2);
612 final DerivativeStructure latDS = factory.build(lat0, lat1, lat2);
613 final DerivativeStructure lonDS = factory.build(lon0, lon1, lon2);
614 final DerivativeStructure altDS = factory.build(alt0, alt1, alt2);
615
616
617 PVCoordinates pv = new PVCoordinates(earth.transform(new FieldGeodeticPoint<>(latDS, lonDS, altDS)));
618 FieldGeodeticPoint<UnivariateDerivative2> rebuilt = earth.transform(pv, earth.getBodyFrame(), AbsoluteDate.ARBITRARY_EPOCH);
619 Assertions.assertEquals(lat0, rebuilt.getLatitude().getReal(), 1.0e-16);
620 Assertions.assertEquals(lat1, rebuilt.getLatitude().getPartialDerivative(1), 5.0e-19);
621 Assertions.assertEquals(lat2, rebuilt.getLatitude().getPartialDerivative(2), 5.0e-14);
622 Assertions.assertEquals(lon0, rebuilt.getLongitude().getReal(), 1.0e-16);
623 Assertions.assertEquals(lon1, rebuilt.getLongitude().getPartialDerivative(1), 1.0e-18);
624 Assertions.assertEquals(lon2, rebuilt.getLongitude().getPartialDerivative(2), 1.0e-20);
625 Assertions.assertEquals(alt0, rebuilt.getAltitude().getReal(), 2.0e-11);
626 Assertions.assertEquals(alt1, rebuilt.getAltitude().getPartialDerivative(1), 1.0e-12);
627 Assertions.assertEquals(alt2, rebuilt.getAltitude().getPartialDerivative(2), 2.0e-14);
628
629 }
630
631 @Test
632 void testIssue873() {
633 AbsoluteDate epoch = AbsoluteDate.ARBITRARY_EPOCH;
634 OneAxisEllipsoid earth = ReferenceEllipsoid.getWgs84(FramesFactory.getGCRF());
635 FieldGeodeticPoint<UnivariateDerivative2> gp = earth.transform(
636 new PVCoordinates(new Vector3D(0, 0, earth.getC()), new Vector3D(0, 1, 0)),
637 earth.getBodyFrame(), epoch);
638 assertThat(gp.getAltitude().getPartialDerivative(1), closeTo(0.0, 1e-12));
639 }
640
641 @Test
642 void testMovingGeodeticPoint() {
643
644 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
645 Constants.WGS84_EARTH_FLATTENING,
646 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
647 double lat0 = FastMath.toRadians(60.0);
648 double lon0 = FastMath.toRadians(25.0);
649 double alt0 = 100.0;
650 double lat1 = 1.0e-3;
651 double lon1 = -2.0e-3;
652 double alt1 = 1.2;
653 double lat2 = -1.0e-5;
654 double lon2 = -3.0e-5;
655 double alt2 = -0.01;
656 final DSFactory factory = new DSFactory(1, 2);
657 final DerivativeStructure latDS = factory.build(lat0, lat1, lat2);
658 final DerivativeStructure lonDS = factory.build(lon0, lon1, lon2);
659 final DerivativeStructure altDS = factory.build(alt0, alt1, alt2);
660
661
662 PVCoordinates pv = new PVCoordinates(earth.transform(new FieldGeodeticPoint<>(latDS, lonDS, altDS)));
663
664
665 FiniteDifferencesDifferentiator differentiator =
666 new FiniteDifferencesDifferentiator(5, 0.1);
667 UnivariateDifferentiableFunction fx =
668 differentiator.differentiate((UnivariateFunction) dt -> {
669 GeodeticPoint gp = new GeodeticPoint(latDS.taylor(dt), lonDS.taylor(dt), altDS.taylor(dt));
670 return earth.transform(gp).getX();
671 });
672 UnivariateDifferentiableFunction fy =
673 differentiator.differentiate((UnivariateFunction) dt -> {
674 GeodeticPoint gp = new GeodeticPoint(latDS.taylor(dt), lonDS.taylor(dt), altDS.taylor(dt));
675 return earth.transform(gp).getY();
676 });
677 UnivariateDifferentiableFunction fz =
678 differentiator.differentiate((UnivariateFunction) dt -> {
679 GeodeticPoint gp = new GeodeticPoint(latDS.taylor(dt), lonDS.taylor(dt), altDS.taylor(dt));
680 return earth.transform(gp).getZ();
681 });
682 DerivativeStructure dtZero = factory.variable(0, 0.0);
683 DerivativeStructure xDS = fx.value(dtZero);
684 DerivativeStructure yDS = fy.value(dtZero);
685 DerivativeStructure zDS = fz.value(dtZero);
686 Assertions.assertEquals(xDS.getValue(), pv.getPosition().getX(),
687 2.0e-20 * FastMath.abs(xDS.getValue()));
688 Assertions.assertEquals(xDS.getPartialDerivative(1), pv.getVelocity().getX(),
689 2.0e-12 * FastMath.abs(xDS.getPartialDerivative(1)));
690 Assertions.assertEquals(xDS.getPartialDerivative(2), pv.getAcceleration().getX(),
691 2.0e-9 * FastMath.abs(xDS.getPartialDerivative(2)));
692 Assertions.assertEquals(yDS.getValue(), pv.getPosition().getY(),
693 2.0e-20 * FastMath.abs(yDS.getValue()));
694 Assertions.assertEquals(yDS.getPartialDerivative(1), pv.getVelocity().getY(),
695 2.0e-12 * FastMath.abs(yDS.getPartialDerivative(1)));
696 Assertions.assertEquals(yDS.getPartialDerivative(2), pv.getAcceleration().getY(),
697 2.0e-9 * FastMath.abs(yDS.getPartialDerivative(2)));
698 Assertions.assertEquals(zDS.getValue(), pv.getPosition().getZ(),
699 2.0e-20 * FastMath.abs(zDS.getValue()));
700 Assertions.assertEquals(zDS.getPartialDerivative(1), pv.getVelocity().getZ(),
701 2.0e-12 * FastMath.abs(zDS.getPartialDerivative(1)));
702 Assertions.assertEquals(zDS.getPartialDerivative(2), pv.getAcceleration().getZ(),
703 2.0e-9 * FastMath.abs(zDS.getPartialDerivative(2)));
704 }
705
706 private void checkCartesianToEllipsoidic(double ae, double f,
707 double x, double y, double z,
708 double longitude, double latitude,
709 double altitude) {
710
711 AbsoluteDate date = AbsoluteDate.J2000_EPOCH;
712 Frame frame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
713 OneAxisEllipsoid model = new OneAxisEllipsoid(ae, f, frame);
714 GeodeticPoint gp = model.transform(new Vector3D(x, y, z), frame, date);
715 Assertions.assertEquals(longitude, MathUtils.normalizeAngle(gp.getLongitude(), longitude), 1.0e-10);
716 Assertions.assertEquals(latitude, gp.getLatitude(), 1.0e-10);
717 Assertions.assertEquals(altitude, gp.getAltitude(), 1.0e-10 * FastMath.abs(ae));
718 Vector3D rebuiltNadir = Vector3D.crossProduct(gp.getSouth(), gp.getWest());
719 Assertions.assertEquals(0, rebuiltNadir.subtract(gp.getNadir()).getNorm(), 1.0e-15);
720
721 FieldGeodeticPoint<Binary64> gp64 = model.transform(new FieldVector3D<>(new Binary64(x),
722 new Binary64(y),
723 new Binary64(z)),
724 frame,
725 new FieldAbsoluteDate<>(Binary64Field.getInstance(), date));
726 Assertions.assertEquals(longitude, MathUtils.normalizeAngle(gp64.getLongitude().getReal(), longitude), 1.0e-10);
727 Assertions.assertEquals(latitude, gp64.getLatitude().getReal(), 1.0e-10);
728 Assertions.assertEquals(altitude, gp64.getAltitude().getReal(), 1.0e-10 * FastMath.abs(ae));
729 FieldVector3D<Binary64> rebuiltNadir64 = FieldVector3D.crossProduct(gp64.getSouth(), gp64.getWest());
730 Assertions.assertEquals(0, rebuiltNadir64.subtract(gp64.getNadir()).getNorm().getReal(), 1.0e-15);
731
732
733 gp = model.transform(model.projectToGround(new Vector3D(x, y, z), date, frame), frame, date);
734 Assertions.assertEquals(longitude, MathUtils.normalizeAngle(gp.getLongitude(), longitude), 1.0e-10);
735 Assertions.assertEquals(latitude, gp.getLatitude(), 1.0e-10);
736 Assertions.assertEquals(0.0, gp.getAltitude(), 1.0e-10 * FastMath.abs(ae));
737
738
739 FieldGeodeticPoint<UnivariateDerivative2> gpDs = model.transform(
740 model.projectToGround(
741 new TimeStampedPVCoordinates(
742 date,
743 new Vector3D(x, y, z),
744 new Vector3D(1, 2, 3)),
745 frame),
746 frame,
747 date);
748 Assertions.assertEquals(longitude, MathUtils.normalizeAngle(gpDs.getLongitude().getReal(), longitude), 1.0e-10);
749 Assertions.assertEquals(latitude, gpDs.getLatitude().getReal(), 1.0e-10);
750 Assertions.assertEquals(0.0, gpDs.getAltitude().getReal(), 1.0e-10 * FastMath.abs(ae));
751
752 }
753
754 @Test
755 void testTransformVsOldIterativeSobol() {
756
757 OneAxisEllipsoid model = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
758 Constants.WGS84_EARTH_FLATTENING,
759 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
760 SobolSequenceGenerator sobol = new SobolSequenceGenerator(3);
761 final double rMax = 10 * model.getEquatorialRadius();
762 Stream<Vector3D> points = Stream.generate(() -> {
763 final double[] v = sobol.nextVector();
764 return new Vector3D(rMax * (2 * v[0] - 1), rMax * (2 * v[1] - 1), rMax * (2 * v[2] - 1));
765 }).limit(1000000);
766
767 doTestTransformVsOldIterative(model, points, 2.0e-15, 1.0e-15, 8.0e-14 * rMax);
768 }
769
770 @Test
771 void testTransformVsOldIterativePolarAxis() {
772 OneAxisEllipsoid model = new OneAxisEllipsoid(90, 5.0 / 9.0,
773 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
774 Stream<Vector3D> points = DoubleStream.iterate(0, x -> x + 1.0).limit(150).mapToObj(z -> new Vector3D(0, 0, z));
775 doTestTransformVsOldIterative(model, points, 2.0e-15, 1.0e-15, 1.0e-14 * model.getEquatorialRadius());
776 }
777
778 @Test
779 void testTransformVsOldIterativeEquatorial() {
780 OneAxisEllipsoid model = new OneAxisEllipsoid(90, 5.0 / 9.0,
781 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
782 Stream<Vector3D> points = DoubleStream.iterate(0, x -> x + 1.0).limit(150).mapToObj(x -> new Vector3D(x, 0, 0));
783 doTestTransformVsOldIterative(model, points, 2.0e-15, 1.0e-15, 1.0e-14 * model.getEquatorialRadius());
784 }
785
786 @Test
787 void testIssue373() {
788 final Frame ecef = FramesFactory.getITRF(IERSConventions.IERS_2010,true);
789 final OneAxisEllipsoid earth = new OneAxisEllipsoid(6378137, 1./298.257223563, ecef);
790 final Vector3D sunPos = new Vector3D(-149757851422.23358, 8410610314.781021, 14717269835.161688 );
791 final GeodeticPoint sunGP = earth.transform(sunPos, ecef, null);
792 Assertions.assertEquals(5.603878, FastMath.toDegrees(sunGP.getLatitude()), 1.0e-6);
793 }
794
795 @Test
796 void testIsometricLatitude() {
797 final Frame ecef = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
798 final OneAxisEllipsoid earth = new OneAxisEllipsoid(6378137, 1. / 298.257223563, ecef);
799
800 Assertions.assertEquals(0., earth.geodeticToIsometricLatitude(0), 1.0e-9);
801 Assertions.assertEquals(0., earth.geodeticToIsometricLatitude(2.0e-13), 1.0e-9);
802
803 for (final double lat: new double[] {
804 FastMath.toRadians(10),
805 FastMath.toRadians(-45),
806 FastMath.toRadians(80),
807 FastMath.toRadians(-90)}) {
808 final double eSinLat = earth.getEccentricity() * FastMath.sin(lat);
809 final double term1 = FastMath.log(FastMath.tan(FastMath.PI / 4. + lat / 2.));
810 final double term2 = (earth.getEccentricity() / 2.) * FastMath.log((1. - eSinLat) / (1 + eSinLat));
811
812 Assertions.assertEquals(term1 + term2, earth.geodeticToIsometricLatitude(lat), 1.0e-12);
813 }
814 }
815
816 @Test
817 void testFieldIsometricLatitude() {
818 final Frame ecef = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
819 final OneAxisEllipsoid earth = new OneAxisEllipsoid(6378137, 1. / 298.257223563, ecef);
820
821 final Binary64Field field = Binary64Field.getInstance();
822 Assertions.assertEquals(Binary64Field.getInstance().getZero(), earth.geodeticToIsometricLatitude(field.getOne().newInstance(0.)));
823 Assertions.assertEquals(Binary64Field.getInstance().getZero(), earth.geodeticToIsometricLatitude(field.getOne().newInstance(2.0e-13)));
824
825 final Binary64 ecc = field.getZero().newInstance(earth.getEccentricity());
826 for (final Binary64 lat: new Binary64[] {
827 field.getOne().newInstance(FastMath.toRadians(10)),
828 field.getOne().newInstance(FastMath.toRadians(-45)),
829 field.getOne().newInstance(FastMath.toRadians(80)),
830 field.getOne().newInstance(FastMath.toRadians(-90))}) {
831 final Binary64 eSinLat = ecc.multiply(FastMath.sin(lat));
832 final Binary64 term1 = FastMath.log(FastMath.tan(lat.getPi().divide(4.).add(lat.divide(2.))));
833 final Binary64 term2 = ecc.divide(2.).multiply(FastMath.log(field.getOne().subtract(eSinLat).divide(field.getOne().add(eSinLat))));
834
835 Assertions.assertEquals(term1.add(term2), earth.geodeticToIsometricLatitude(lat));
836 }
837 }
838
839 @Test
840 void testAzimuthBetweenPoints() {
841 final Frame ecef = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
842 final OneAxisEllipsoid earth = new OneAxisEllipsoid(6378137, 1. / 298.257223563, ecef);
843
844
845 final GeodeticPoint newYork = new GeodeticPoint(FastMath.toRadians(40.71427), FastMath.toRadians(-74.00597), 0);
846 final GeodeticPoint chicago = new GeodeticPoint(FastMath.toRadians(41.85003), FastMath.toRadians(-87.65005), 0);
847 final GeodeticPoint london = new GeodeticPoint(FastMath.toRadians(51.5), FastMath.toRadians(-0.16667), 0);
848 final GeodeticPoint berlin = new GeodeticPoint(FastMath.toRadians(52.523403), FastMath.toRadians(13.4114), 0);
849 final GeodeticPoint perth = new GeodeticPoint(FastMath.toRadians(-31.952712), FastMath.toRadians(115.8604796), 0);
850
851
852 Assertions.assertEquals(276.297499, FastMath.toDegrees(earth.azimuthBetweenPoints(newYork, chicago)), 1.0e-6);
853 Assertions.assertEquals(78.0854898, FastMath.toDegrees(earth.azimuthBetweenPoints(newYork, london)), 1.0e-6);
854 Assertions.assertEquals(83.0357553, FastMath.toDegrees(earth.azimuthBetweenPoints(london, berlin)), 1.0e-6);
855 Assertions.assertEquals(132.894864, FastMath.toDegrees(earth.azimuthBetweenPoints(berlin, perth)), 1.0e-6);
856 Assertions.assertEquals(65.3853195, FastMath.toDegrees(earth.azimuthBetweenPoints(perth, newYork)), 1.0e-6);
857 }
858
859 @Test
860 void testAzimuthBetweenFieldPoints() {
861 final Frame ecef = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
862 final OneAxisEllipsoid earth = new OneAxisEllipsoid(6378137, 1. / 298.257223563, ecef);
863
864 final Binary64Field field = Binary64Field.getInstance();
865
866
867 final FieldGeodeticPoint<Binary64> newYork = new FieldGeodeticPoint<>(
868 FastMath.toRadians(field.getZero().add(40.71427)),
869 FastMath.toRadians(field.getZero().add(-74.00597)), field.getZero());
870 final FieldGeodeticPoint<Binary64> chicago = new FieldGeodeticPoint<>(
871 FastMath.toRadians(field.getZero().add(41.85003)),
872 FastMath.toRadians(field.getZero().add(-87.65005)), field.getZero());
873
874 final FieldGeodeticPoint<Binary64> london = new FieldGeodeticPoint<>(
875 FastMath.toRadians(field.getZero().add(51.5)),
876 FastMath.toRadians(field.getZero().add(-0.16667)), field.getZero());
877 final FieldGeodeticPoint<Binary64> berlin = new FieldGeodeticPoint<>(
878 FastMath.toRadians(field.getZero().add(52.523403)),
879 FastMath.toRadians(field.getZero().add(13.4114)), field.getZero());
880 final FieldGeodeticPoint<Binary64> perth = new FieldGeodeticPoint<>(
881 FastMath.toRadians(field.getZero().add(-31.952712)),
882 FastMath.toRadians(field.getZero().add(115.8604796)), field.getZero());
883
884
885 Assertions.assertEquals(276.297499, FastMath.toDegrees(earth.azimuthBetweenPoints(newYork, chicago)).getReal(), 1.0e-6);
886 Assertions.assertEquals(78.0854898, FastMath.toDegrees(earth.azimuthBetweenPoints(newYork, london)).getReal(), 1.0e-6);
887 Assertions.assertEquals(83.0357553, FastMath.toDegrees(earth.azimuthBetweenPoints(london, berlin)).getReal(), 1.0e-6);
888 Assertions.assertEquals(132.894864, FastMath.toDegrees(earth.azimuthBetweenPoints(berlin, perth)).getReal(), 1.0e-6);
889 Assertions.assertEquals(65.3853195, FastMath.toDegrees(earth.azimuthBetweenPoints(perth, newYork)).getReal(), 1.0e-6);
890 }
891
892 @Test
893 void testPointNearCenter1() {
894 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
895 Constants.WGS84_EARTH_FLATTENING,
896 FramesFactory.getITRF(IERSConventions.IERS_2010, false));
897 final Vector3D p1 = new Vector3D( 14605530.402633, 7681001.886684, 24582223.005261);
898 final Vector3D p2 = new Vector3D(-14650836.411867, -7561887.405778, -24575352.170908);
899 final Vector3D pMid = new Vector3D(0.5, p1, 0.5, p2);
900 final GeodeticPoint gp = earth.transform(pMid, earth.getFrame(), null);
901 Vector3D rebuilt = earth.transform(gp);
902 Assertions.assertEquals(0.0, rebuilt.distance(pMid), 1.5e-9);
903 }
904
905 @Test
906 void testPointNearCenter2() {
907 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
908 Constants.WGS84_EARTH_FLATTENING,
909 FramesFactory.getITRF(IERSConventions.IERS_2010, false));
910 final Vector3D pMid = new Vector3D(-20923.23737959098, 56464.586571323685, -7647.317096056417);
911 final GeodeticPoint gp = earth.transform(pMid, earth.getFrame(), null);
912 Vector3D rebuilt = earth.transform(gp);
913
914 Assertions.assertEquals(540.598, rebuilt.distance(pMid), 1.0e-3);
915 }
916
917
918 @Test
919 void testLowestAltitudeIntermediate() {
920 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
921 Constants.WGS84_EARTH_FLATTENING,
922 FramesFactory.getITRF(IERSConventions.IERS_2010, false));
923 final Vector3D close = earth.transform(new GeodeticPoint(FastMath.toRadians(12.0),
924 FastMath.toRadians(34.0),
925 100000.0));
926 final Vector3D far = earth.transform(new GeodeticPoint(FastMath.toRadians(-47.0),
927 FastMath.toRadians( 43.0),
928 10000000.0));
929 final GeodeticPoint lowestA = earth.lowestAltitudeIntermediate(close, far);
930 Assertions.assertEquals(0.0, Vector3D.distance(close, earth.transform(lowestA)), 2.0e-9);
931 final GeodeticPoint lowestB = earth.lowestAltitudeIntermediate(far, close);
932 Assertions.assertEquals(0.0, Vector3D.distance(close, earth.transform(lowestB)), 2.0e-9);
933
934 final double h = 5000000.0;
935 final Vector3D p1 = earth.transform(new GeodeticPoint(FastMath.toRadians(-20.0),
936 FastMath.toRadians(-12.0),
937 h));
938 final Vector3D p2 = earth.transform(new GeodeticPoint(FastMath.toRadians(17.0),
939 FastMath.toRadians(13.0),
940 h));
941 final GeodeticPoint lowest = earth.lowestAltitudeIntermediate(p1, p2);
942 Assertions.assertTrue(lowest.getAltitude() < h);
943 final GeodeticPoint oneCentimeterBefore = earth.transform(new Vector3D( 1.0, earth.transform(lowest),
944 -1.0e-2, p2.subtract(p1).normalize()),
945 earth.getBodyFrame(), null);
946 Assertions.assertTrue(oneCentimeterBefore.getAltitude() > lowest.getAltitude());
947 final GeodeticPoint oneCentimeterAfter = earth.transform(new Vector3D( 1.0, earth.transform(lowest),
948 +1.0e-2, p2.subtract(p1).normalize()),
949 earth.getBodyFrame(), null);
950 Assertions.assertTrue(oneCentimeterAfter.getAltitude() > lowest.getAltitude());
951
952 }
953
954 @Test
955 void testLowestAltitudeIntermediateField() {
956 doTestLowestAltitudeIntermediateField(Binary64Field.getInstance());
957 }
958
959 private <T extends CalculusFieldElement<T>> void doTestLowestAltitudeIntermediateField(final Field<T> field) {
960
961 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
962 Constants.WGS84_EARTH_FLATTENING,
963 FramesFactory.getITRF(IERSConventions.IERS_2010, false));
964 final FieldVector3D<T> close = earth.transform(new FieldGeodeticPoint<>(FastMath.toRadians(field.getZero().newInstance(12.0)),
965 FastMath.toRadians(field.getZero().newInstance(34.0)),
966 field.getZero().newInstance(100000.0)));
967 final FieldVector3D<T> far = earth.transform(new FieldGeodeticPoint<>(FastMath.toRadians(field.getZero().newInstance(-47.0)),
968 FastMath.toRadians(field.getZero().newInstance( 43.0)),
969 field.getZero().newInstance(10000000.0)));
970 final FieldGeodeticPoint<T> lowestA = earth.lowestAltitudeIntermediate(close, far);
971 Assertions.assertEquals(0.0, FieldVector3D.distance(close, earth.transform(lowestA)).getReal(), 2.0e-9);
972 final FieldGeodeticPoint<T> lowestB = earth.lowestAltitudeIntermediate(far, close);
973 Assertions.assertEquals(0.0, FieldVector3D.distance(close, earth.transform(lowestB)).getReal(), 2.0e-9);
974
975 final double h =5000000.0;
976 final FieldVector3D<T> p1 = earth.transform(new FieldGeodeticPoint<>(FastMath.toRadians(field.getZero().newInstance(-20.0)),
977 FastMath.toRadians(field.getZero().newInstance(-12.0)),
978 field.getZero().newInstance(h)));
979 final FieldVector3D<T> p2 = earth.transform(new FieldGeodeticPoint<>(FastMath.toRadians(field.getZero().newInstance(17.0)),
980 FastMath.toRadians(field.getZero().newInstance(13.0)),
981 field.getZero().newInstance(h)));
982 final FieldGeodeticPoint<T> lowest = earth.lowestAltitudeIntermediate(p1, p2);
983 Assertions.assertTrue(lowest.getAltitude().getReal() < h);
984 final FieldGeodeticPoint<T> oneCentimeterBefore = earth.transform(new FieldVector3D<>( 1.0, earth.transform(lowest),
985 -1.0e-2, p2.subtract(p1).normalize()),
986 earth.getBodyFrame(), null);
987 Assertions.assertTrue(oneCentimeterBefore.getAltitude().subtract(lowest.getAltitude()).getReal() > 0);
988 final FieldGeodeticPoint<T> oneCentimeterAfter = earth.transform(new FieldVector3D<>( 1.0, earth.transform(lowest),
989 +1.0e-2, p2.subtract(p1).normalize()),
990 earth.getBodyFrame(), null);
991 Assertions.assertTrue(oneCentimeterAfter.getAltitude().subtract(lowest.getAltitude()).getReal() > 0);
992
993 }
994
995 @Test
996 public void testPointAtAltitude() {
997 final Frame inertial = FramesFactory.getGCRF();
998 final AbsoluteDate date = new AbsoluteDate(2003, 5, 7, 12, 34, 0.0, TimeScalesFactory.getUTC());
999 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
1000 Constants.WGS84_EARTH_FLATTENING,
1001 FramesFactory.getITRF(IERSConventions.IERS_2010, false));
1002 final GeodeticPoint wenoGP = new GeodeticPoint(FastMath.toRadians( 7.4423),
1003 FastMath.toRadians(151.8398),
1004 300000.0);
1005 final StaticTransform earth2Inertial = earth.getBodyFrame().getStaticTransformTo(inertial, date);
1006 final Vector3D weno3D = earth2Inertial.transformPosition(earth.transform(wenoGP));
1007 final GeodeticPoint tuvaluGP = new GeodeticPoint(FastMath.toRadians(-8.515194),
1008 FastMath.toRadians(179.199420),
1009 3500000.0);
1010 final Vector3D tuvalu3D = earth2Inertial.transformPosition(earth.transform(tuvaluGP));
1011 final Line lineA = new Line(tuvalu3D, weno3D, 1.0e-12);
1012 final Vector3D resA = earth.pointAtAltitude(lineA, wenoGP.getAltitude(), tuvalu3D, inertial, date);
1013 Assertions.assertEquals(0, resA.distance(weno3D), 1.0e-6);
1014
1015 final Vector3D groundA1I = earth.pointAtAltitude(lineA, 0, tuvalu3D, inertial, date);
1016 final Vector3D groundA2E = earth.getCartesianIntersectionPoint(lineA, tuvalu3D, inertial, date);
1017 final Vector3D groundA2I = earth2Inertial.transformPosition(groundA2E);
1018 Assertions.assertEquals(0, Vector3D.distance(groundA1I, groundA2I), 1.0e-6);
1019
1020 final Line lineB = new Line(weno3D, tuvalu3D, 1.0e-12);
1021 final Vector3D resB = earth.pointAtAltitude(lineB, wenoGP.getAltitude(), tuvalu3D, inertial, date);
1022 Assertions.assertEquals(0, resB.distance(weno3D), 1.0e-6);
1023
1024 final Vector3D groundB1I = earth.pointAtAltitude(lineB, 0, tuvalu3D, inertial, date);
1025 final Vector3D groundB2E = earth.getCartesianIntersectionPoint(lineB, tuvalu3D, inertial, date);
1026 final Vector3D groundB2I = earth2Inertial.transformPosition(groundB2E);
1027 Assertions.assertEquals(0, Vector3D.distance(groundB1I, groundB2I), 1.0e-6);
1028
1029 }
1030
1031 @Test
1032 public void testPointAtAltitudeNoCrossing() {
1033 final Frame inertial = FramesFactory.getGCRF();
1034 final AbsoluteDate date = new AbsoluteDate(2003, 5, 7, 12, 34, 0.0, TimeScalesFactory.getUTC());
1035 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
1036 Constants.WGS84_EARTH_FLATTENING,
1037 FramesFactory.getITRF(IERSConventions.IERS_2010, false));
1038 final GeodeticPoint wenoGP = new GeodeticPoint(FastMath.toRadians( 7.4423),
1039 FastMath.toRadians(151.8398),
1040 500000.0);
1041 final StaticTransform earth2Inertial = earth.getBodyFrame().getStaticTransformTo(inertial, date);
1042 final Vector3D weno3D = earth2Inertial.transformPosition(earth.transform(wenoGP));
1043 final GeodeticPoint tuvaluGP = new GeodeticPoint(FastMath.toRadians(-8.515194),
1044 FastMath.toRadians(179.199420),
1045 800000.0);
1046 final Vector3D tuvalu3D = earth2Inertial.transformPosition(earth.transform(tuvaluGP));
1047 final Line lineA = new Line(tuvalu3D, weno3D, 1.0e-12);
1048 try {
1049 earth.pointAtAltitude(lineA, 200000.0, tuvalu3D, inertial, date);
1050 Assertions.fail("an exception should have been thrown");
1051 } catch (OrekitException ex) {
1052 Assertions.assertEquals(OrekitMessages.LINE_NEVER_CROSSES_ALTITUDE, ex.getSpecifier());
1053 Assertions.assertEquals(200000.0, (Double) ex.getParts()[0], 1.0e-6);
1054 }
1055
1056 }
1057
1058 @Test
1059 public void testPointAtAltitudeNoConvergence() {
1060 final Frame inertial = FramesFactory.getGCRF();
1061 final AbsoluteDate date = new AbsoluteDate(2003, 5, 7, 12, 34, 0.0, TimeScalesFactory.getUTC());
1062 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
1063 Constants.WGS84_EARTH_FLATTENING,
1064 FramesFactory.getITRF(IERSConventions.IERS_2010, false));
1065 final GeodeticPoint skimmingGP = new GeodeticPoint(FastMath.toRadians(1.6498654859),
1066 FastMath.toRadians(161.7837673842),
1067 362673.428653);
1068 final StaticTransform earth2Inertial = earth.getBodyFrame().getStaticTransformTo(inertial, date);
1069 final Vector3D skimming3D = earth2Inertial.transformPosition(earth.transform(skimmingGP));
1070 final GeodeticPoint tuvaluGP = new GeodeticPoint(FastMath.toRadians(-8.515194),
1071 FastMath.toRadians(179.199420),
1072 800000.0);
1073 final Vector3D tuvalu3D = earth2Inertial.transformPosition(earth.transform(tuvaluGP));
1074 final Line lineA = new Line(tuvalu3D, skimming3D, 1.0e-12);
1075 try {
1076 earth.pointAtAltitude(lineA, skimmingGP.getAltitude() - 1.0e-6, tuvalu3D, inertial, date);
1077 Assertions.fail("an exception should have been thrown");
1078 } catch (OrekitException ex) {
1079 Assertions.assertEquals(LocalizedCoreFormats.CONVERGENCE_FAILED, ex.getSpecifier());
1080 }
1081
1082 }
1083
1084 @Test
1085 public void testFieldPointAtAltitude() {
1086 doTestFieldPointAtAltitude(Binary64Field.getInstance());
1087 }
1088
1089 private <T extends CalculusFieldElement<T>> void doTestFieldPointAtAltitude(final Field<T> field) {
1090 final Frame inertial = FramesFactory.getGCRF();
1091 final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field,
1092 new AbsoluteDate(2003, 5, 7, 12, 34, 0.0,
1093 TimeScalesFactory.getUTC()));
1094 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
1095 Constants.WGS84_EARTH_FLATTENING,
1096 FramesFactory.getITRF(IERSConventions.IERS_2010, false));
1097 final FieldGeodeticPoint<T> wenoGP = new FieldGeodeticPoint<>(field,
1098 new GeodeticPoint(FastMath.toRadians(7.4423),
1099 FastMath.toRadians(151.8398),
1100 300000.0));
1101 final FieldStaticTransform<T> earth2Inertial = earth.getBodyFrame().getStaticTransformTo(inertial, date);
1102 final FieldVector3D<T> weno3D = earth2Inertial.transformPosition(earth.transform(wenoGP));
1103 final FieldGeodeticPoint<T> tuvaluGP = new FieldGeodeticPoint<>(field,
1104 new GeodeticPoint(FastMath.toRadians(-8.515194),
1105 FastMath.toRadians(179.199420),
1106 3500000.0));
1107 final FieldVector3D<T> tuvalu3D = earth2Inertial.transformPosition(earth.transform(tuvaluGP));
1108 final FieldLine<T> lineA = new FieldLine<>(tuvalu3D, weno3D, 1.0e-12);
1109 final FieldVector3D<T> resA = earth.pointAtAltitude(lineA, wenoGP.getAltitude(), tuvalu3D, inertial, date);
1110 Assertions.assertEquals(0, resA.distance(weno3D).getReal(), 1.0e-6);
1111
1112 final FieldVector3D<T> groundA1I = earth.pointAtAltitude(lineA, field.getZero(), tuvalu3D, inertial, date);
1113 final FieldVector3D<T> groundA2E = earth.getCartesianIntersectionPoint(lineA, tuvalu3D, inertial, date);
1114 final FieldVector3D<T> groundA2I = earth2Inertial.transformPosition(groundA2E);
1115 Assertions.assertEquals(0, FieldVector3D.distance(groundA1I, groundA2I).getReal(), 1.0e-6);
1116
1117 final FieldLine<T> lineB = new FieldLine<>(weno3D, tuvalu3D, 1.0e-12);
1118 final FieldVector3D<T> resB = earth.pointAtAltitude(lineB, wenoGP.getAltitude(), tuvalu3D, inertial, date);
1119 Assertions.assertEquals(0, resB.distance(weno3D).getReal(), 1.0e-6);
1120
1121 final FieldVector3D<T> groundB1I = earth.pointAtAltitude(lineB, field.getZero(), tuvalu3D, inertial, date);
1122 final FieldVector3D<T> groundB2E = earth.getCartesianIntersectionPoint(lineB, tuvalu3D, inertial, date);
1123 final FieldVector3D<T> groundB2I = earth2Inertial.transformPosition(groundB2E);
1124 Assertions.assertEquals(0, FieldVector3D.distance(groundB1I, groundB2I).getReal(), 1.0e-6);
1125
1126 }
1127
1128 @Test
1129 public void testFieldPointAtAltitudeNoCrossing() {
1130 doTestFieldPointAtAltitudeNoCrossing(Binary64Field.getInstance());
1131 }
1132
1133 private <T extends CalculusFieldElement<T>> void doTestFieldPointAtAltitudeNoCrossing(final Field<T> field) {
1134 final Frame inertial = FramesFactory.getGCRF();
1135 final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field,
1136 new AbsoluteDate(2003, 5, 7, 12, 34, 0.0,
1137 TimeScalesFactory.getUTC()));
1138 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
1139 Constants.WGS84_EARTH_FLATTENING,
1140 FramesFactory.getITRF(IERSConventions.IERS_2010, false));
1141 final FieldGeodeticPoint<T> wenoGP = new FieldGeodeticPoint<>(field,
1142 new GeodeticPoint(FastMath.toRadians(7.4423),
1143 FastMath.toRadians(151.8398),
1144 500000.0));
1145 final FieldStaticTransform<T> earth2Inertial = earth.getBodyFrame().getStaticTransformTo(inertial, date);
1146 final FieldVector3D<T> weno3D = earth2Inertial.transformPosition(earth.transform(wenoGP));
1147 final FieldGeodeticPoint<T> tuvaluGP = new FieldGeodeticPoint<>(field,
1148 new GeodeticPoint(FastMath.toRadians(-8.515194),
1149 FastMath.toRadians(179.199420),
1150 800000.0));
1151 final FieldVector3D<T> tuvalu3D = earth2Inertial.transformPosition(earth.transform(tuvaluGP));
1152 final FieldLine<T> lineA = new FieldLine<>(tuvalu3D, weno3D, 1.0e-12);
1153 try {
1154 earth.pointAtAltitude(lineA, field.getZero().newInstance(200000.0), tuvalu3D, inertial, date);
1155 Assertions.fail("an exception should have been thrown");
1156 } catch (OrekitException ex) {
1157 Assertions.assertEquals(OrekitMessages.LINE_NEVER_CROSSES_ALTITUDE, ex.getSpecifier());
1158 Assertions.assertEquals(200000.0, (Double) ex.getParts()[0], 1.0e-6);
1159 }
1160
1161 }
1162
1163 @Test
1164 public void testFieldPointAtAltitudeNoConvergence() {
1165 doTestFieldPointAtAltitudeNoConvergence(Binary64Field.getInstance());
1166 }
1167
1168 private <T extends CalculusFieldElement<T>> void doTestFieldPointAtAltitudeNoConvergence(final Field<T> field) {
1169 final Frame inertial = FramesFactory.getGCRF();
1170 final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field,
1171 new AbsoluteDate(2003, 5, 7, 12, 34, 0.0,
1172 TimeScalesFactory.getUTC()));
1173 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
1174 Constants.WGS84_EARTH_FLATTENING,
1175 FramesFactory.getITRF(IERSConventions.IERS_2010, false));
1176 final FieldGeodeticPoint<T> skimmingGP = new FieldGeodeticPoint<>(field,
1177 new GeodeticPoint(FastMath.toRadians(1.6498654859),
1178 FastMath.toRadians(161.7837673842),
1179 362673.428653));
1180 final FieldStaticTransform<T> earth2Inertial = earth.getBodyFrame().getStaticTransformTo(inertial, date);
1181 final FieldVector3D<T> skimming3D = earth2Inertial.transformPosition(earth.transform(skimmingGP));
1182 final FieldGeodeticPoint<T> tuvaluGP = new FieldGeodeticPoint<>(field,
1183 new GeodeticPoint(FastMath.toRadians(-8.515194),
1184 FastMath.toRadians(179.199420),
1185 800000.0));
1186 final FieldVector3D<T> tuvalu3D = earth2Inertial.transformPosition(earth.transform(tuvaluGP));
1187 final FieldLine<T> lineA = new FieldLine<>(tuvalu3D, skimming3D, 1.0e-12);
1188 try {
1189 earth.pointAtAltitude(lineA, skimmingGP.getAltitude().subtract(1.0e-6), tuvalu3D, inertial, date);
1190 Assertions.fail("an exception should have been thrown");
1191 } catch (OrekitException ex) {
1192 Assertions.assertEquals(LocalizedCoreFormats.CONVERGENCE_FAILED, ex.getSpecifier());
1193 }
1194
1195 }
1196
1197 private void doTestTransformVsOldIterative(OneAxisEllipsoid model,
1198 Stream<Vector3D> points,
1199 double latitudeTolerance, double longitudeTolerance,
1200 double altitudeTolerance) {
1201 points.forEach(point -> {
1202 try {
1203 GeodeticPoint reference = transformOldIterative(model, point, model.getBodyFrame(), null);
1204 GeodeticPoint result = model.transform(point, model.getBodyFrame(), null);
1205 Assertions.assertEquals(reference.getLatitude(), result.getLatitude(), latitudeTolerance);
1206 Assertions.assertEquals(reference.getLongitude(), result.getLongitude(), longitudeTolerance);
1207 Assertions.assertEquals(reference.getAltitude(), result.getAltitude(), altitudeTolerance);
1208 } catch (OrekitException oe) {
1209 Assertions.fail(oe.getLocalizedMessage());
1210 }
1211 });
1212
1213 }
1214
1215
1216
1217
1218
1219
1220
1221
1222
1223
1224
1225
1226
1227 private GeodeticPoint transformOldIterative(final OneAxisEllipsoid model,
1228 final Vector3D point,
1229 final Frame frame,
1230 final AbsoluteDate date) {
1231
1232
1233 final Vector3D pointInBodyFrame = frame.getTransformTo(model.getBodyFrame(), date).transformPosition(point);
1234 final double r2 = pointInBodyFrame.getX() * pointInBodyFrame.getX() +
1235 pointInBodyFrame.getY() * pointInBodyFrame.getY();
1236 final double r = FastMath.sqrt(r2);
1237 final double z = pointInBodyFrame.getZ();
1238
1239
1240 final Ellipse meridian = new Ellipse(Vector3D.ZERO,
1241 new Vector3D(pointInBodyFrame.getX() / r, pointInBodyFrame.getY() / r, 0),
1242 Vector3D.PLUS_K,
1243 model.getA(), model.getC(), model.getBodyFrame());
1244
1245
1246 final Vector2D ellipsePoint = meridian.projectToEllipse(new Vector2D(r, z));
1247
1248
1249 final double dr = r - ellipsePoint.getX();
1250 final double dz = z - ellipsePoint.getY();
1251 final double ae2 = model.getA() * model.getA();
1252 final double f = model.getFlattening();
1253 final double g = 1.0 - f;
1254 final double g2 = g * g;
1255 final double insideIfNegative = g2 * (r2 - ae2) + z * z;
1256
1257 return new GeodeticPoint(FastMath.atan2(ellipsePoint.getY(), g2 * ellipsePoint.getX()),
1258 FastMath.atan2(pointInBodyFrame.getY(), pointInBodyFrame.getX()),
1259 FastMath.copySign(FastMath.hypot(dr, dz), insideIfNegative));
1260
1261 }
1262
1263 @BeforeEach
1264 public void setUp() {
1265 Utils.setDataRoot("regular-data");
1266 }
1267
1268 }
1269