diff --git a/src/main/java/org/orekit/rugged/utils/ExtendedEllipsoid.java b/src/main/java/org/orekit/rugged/utils/ExtendedEllipsoid.java
index af7906f27865a1a6d89c9e2aab12de3e48e398d6..d44fc18ec635011a7074d854ba651c7cbac86d82 100644
--- a/src/main/java/org/orekit/rugged/utils/ExtendedEllipsoid.java
+++ b/src/main/java/org/orekit/rugged/utils/ExtendedEllipsoid.java
@@ -19,6 +19,7 @@ package org.orekit.rugged.utils;
import org.apache.commons.math3.geometry.euclidean.threed.Line;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math3.util.FastMath;
+import org.apache.commons.math3.util.MathArrays;
import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.errors.OrekitException;
@@ -89,31 +90,48 @@ public class ExtendedEllipsoid extends OneAxisEllipsoid {
DumpManager.dumpEllipsoid(this);
// find apex of iso-latitude cone, somewhere along polar axis
- final GeodeticPoint groundPoint = new GeodeticPoint(latitude, 0, 0);
- final Vector3D gpCartesian = transform(groundPoint);
- final double r = FastMath.sqrt(gpCartesian.getX() * gpCartesian.getX() +
- gpCartesian.getY() * gpCartesian.getY());
- final Vector3D zenith = groundPoint.getZenith();
- final Vector3D apex = new Vector3D(1, gpCartesian, -r / FastMath.cos(latitude), zenith);
+ final double sinPhi = FastMath.sin(latitude);
+ final double sinPhi2 = sinPhi * sinPhi;
+ final double e2 = getFlattening() * (2 - getFlattening());
+ final double apexZ = -getA() * e2 * sinPhi / FastMath.sqrt(1 - e2 * sinPhi2);
// quadratic equation representing line intersection with iso-latitude cone
// a k² + 2 b k + c = 0
- final Vector3D delta = position.subtract(apex);
- final double zz2 = zenith.getZ() * zenith.getZ();
- final double a = zz2 * los.getNormSq() - los.getZ() * los.getZ();
- final double b = zz2 * Vector3D.dotProduct(delta, los) - delta.getZ() * los.getZ();
- final double c = zz2 * delta.getNormSq() - delta.getZ() * delta.getZ();
+ // when line of sight is almost along an iso-latitude generatrix, the quadratic
+ // equation above may become unsolvable due to numerical noise (we get catastrophic
+ // cancellation when computing b * b - a * c). So we set up the model in two steps,
+ // first computing searching k₀ such that a₀ k₀² + 2 b₀ k₀ + c₀, and then using
+ // position + kâ‚€ los as the new initial position, which should lie between the
+ // expected 2 roots
+ final double cosPhi = FastMath.cos(latitude);
+ final double cosPhi2 = cosPhi * cosPhi;
+ final Vector3D delta0 = new Vector3D(position.getX(), position.getY(), position.getZ() - apexZ);
+ final double a0 = MathArrays.linearCombination(+sinPhi2, los.getX() * los.getX() + los.getY() * los.getY(),
+ -cosPhi2, los.getZ() * los.getZ());
+ final double b0 = MathArrays.linearCombination(+sinPhi2, MathArrays.linearCombination(delta0.getX(), los.getX(),
+ delta0.getY(), los.getY()),
+ -cosPhi2, delta0.getZ() * los.getZ());
+ final double k0 = -b0 / a0;
+
+ final Vector3D delta = new Vector3D(MathArrays.linearCombination(1, position.getX(), k0, los.getX()),
+ MathArrays.linearCombination(1, position.getY(), k0, los.getY()),
+ MathArrays.linearCombination(1, position.getZ(), k0, los.getZ(), -1.0, apexZ));
+ final double b = MathArrays.linearCombination(+sinPhi2, MathArrays.linearCombination(delta.getX(), los.getX(),
+ delta.getY(), los.getY()),
+ -cosPhi2, delta.getZ() * los.getZ());
+ final double c = MathArrays.linearCombination(+sinPhi2, delta.getX() * delta.getX() + delta.getY() * delta.getY(),
+ -cosPhi2, delta.getZ() * delta.getZ());
// find the two intersections along the line
- final double bb = b * b;
- final double ac = a * c;
+ final double bb = b * b;
+ final double ac = a0 * c;
if (bb < ac) {
throw new RuggedException(RuggedMessages.LINE_OF_SIGHT_NEVER_CROSSES_LATITUDE,
FastMath.toDegrees(latitude));
}
final double s = FastMath.sqrt(bb - ac);
- final double k1 = (b > 0) ? -(s + b) / a : c / (s - b);
- final double k2 = c / (a * k1);
+ final double k1 = (b > 0) ? -(s + b) / a0 : c / (s - b);
+ final double k2 = c / (a0 * k1);
// the quadratic equation has two solutions
final boolean k1IsOK = (delta.getZ() + k1 * los.getZ()) * latitude >= 0;
@@ -124,7 +142,7 @@ public class ExtendedEllipsoid extends OneAxisEllipsoid {
// both solutions are in the good nappe,
// select the one closest to the specified reference
final double kRef = Vector3D.dotProduct(los, closeReference.subtract(position)) /
- los.getNormSq();
+ los.getNormSq() - k0;
selectedK = FastMath.abs(k1 - kRef) <= FastMath.abs(k2 - kRef) ? k1 : k2;
} else {
// only k1 is in the good nappe
@@ -143,7 +161,7 @@ public class ExtendedEllipsoid extends OneAxisEllipsoid {
}
// compute point
- return new Vector3D(1, position, selectedK, los);
+ return new Vector3D(1, position, k0 + selectedK, los);
}
diff --git a/src/test/java/org/orekit/rugged/api/RuggedTest.java b/src/test/java/org/orekit/rugged/api/RuggedTest.java
index c134d1fe15d66b2b1ce749b0873f694f02036c89..00252890401605ca4dee585734ea44c9d7c237e5 100644
--- a/src/test/java/org/orekit/rugged/api/RuggedTest.java
+++ b/src/test/java/org/orekit/rugged/api/RuggedTest.java
@@ -655,10 +655,10 @@ public class RuggedTest {
@Test
public void testInverseLocation()
throws RuggedException, OrekitException, URISyntaxException {
- checkInverseLocation(2000, false, false, 7.0e-6, 6.0e-6);
- checkInverseLocation(2000, false, true, 1.0e-5, 8.0e-4);
- checkInverseLocation(2000, true, false, 6.0e-6, 5.0e-3);
- checkInverseLocation(2000, true, true, 8.0e-6, 9.0e-4);
+ checkInverseLocation(2000, false, false, 3.0e-7, 5.0e-6);
+ checkInverseLocation(2000, false, true, 6.0e-6, 2.0e-7);
+ checkInverseLocation(2000, true, false, 4.0e-7, 4.0e-7);
+ checkInverseLocation(2000, true, true, 2.0e-5, 3.0e-7);
}
@Test
diff --git a/src/test/java/org/orekit/rugged/errors/DumpReplayerTest.java b/src/test/java/org/orekit/rugged/errors/DumpReplayerTest.java
index 3a45b628c0a2670c1ebdb7defaef7bbb631150de..572a74f2753081eb01bf8d866b4fc76e7ca293d7 100644
--- a/src/test/java/org/orekit/rugged/errors/DumpReplayerTest.java
+++ b/src/test/java/org/orekit/rugged/errors/DumpReplayerTest.java
@@ -56,6 +56,29 @@ public class DumpReplayerTest {
}
+ @Test
+ public void testDirectLoc02() throws URISyntaxException, IOException, OrekitException, RuggedException {
+
+ String orekitPath = getClass().getClassLoader().getResource("orekit-data").toURI().getPath();
+ DataProvidersManager.getInstance().addProvider(new DirectoryCrawler(new File(orekitPath)));
+
+ String dumpPath = getClass().getClassLoader().getResource("replay/replay-direct-loc-02.txt").toURI().getPath();
+ DumpReplayer replayer = new DumpReplayer();
+ replayer.parse(new File(dumpPath));
+ Rugged rugged = replayer.createRugged();
+ DumpReplayer.Result[] results = replayer.execute(rugged);
+
+ Assert.assertEquals(1, results.length);
+ for (final DumpReplayer.Result result : results) {
+ GeodeticPoint expectedGP = (GeodeticPoint) result.getExpected();
+ GeodeticPoint replayedGP = (GeodeticPoint) result.getReplayed();
+ double distance = Vector3D.distance(rugged.getEllipsoid().transform(expectedGP),
+ rugged.getEllipsoid().transform(replayedGP));
+ Assert.assertEquals(0.0, distance, 1.0e-8);
+ }
+
+ }
+
@Test
public void testInverseLoc01() throws URISyntaxException, IOException, OrekitException, RuggedException {
diff --git a/src/test/java/org/orekit/rugged/utils/ExtendedEllipsoidTest.java b/src/test/java/org/orekit/rugged/utils/ExtendedEllipsoidTest.java
index 9f7b4ea7f4a5daf7d0a16119f097a9358f6aa10a..806a809561324644eb3b7410f769d3d993613c6f 100644
--- a/src/test/java/org/orekit/rugged/utils/ExtendedEllipsoidTest.java
+++ b/src/test/java/org/orekit/rugged/utils/ExtendedEllipsoidTest.java
@@ -78,10 +78,12 @@ public class ExtendedEllipsoidTest {
Vector3D p = new Vector3D(3220103.0, 69623.0, -6449822.0);
Vector3D d = new Vector3D(1.0, 2.0, 3.0);
- for (double latitude = -d.getDelta() + 1.0e-6; latitude < d.getDelta(); latitude += 0.1) {
+ double epsilon = 7.0e-8; // will be used only for the first point, as intersection is almost along a generatrix
+ for (double latitude = -d.getDelta() + 1.0e-5; latitude < d.getDelta(); latitude += 0.1) {
GeodeticPoint gp = ellipsoid.transform(ellipsoid.pointAtLatitude(p, d, latitude, p),
ellipsoid.getBodyFrame(), null);
- Assert.assertEquals(latitude, gp.getLatitude(), 1.0e-12);
+ Assert.assertEquals(latitude, gp.getLatitude(), epsilon);
+ epsilon = 1.0e-15; // after first point, far from the generatrix, intersection is very accurate
}
}
@@ -141,6 +143,18 @@ public class ExtendedEllipsoidTest {
}
+ @Test
+ public void testPointAtLatitudeAlmostEquator() throws RuggedException, OrekitException {
+ Vector3D p = new Vector3D(5767483.098580201, 4259689.325372237, -41553.67750784925);
+ Vector3D d = new Vector3D(-0.7403523952347795, -0.6701811835520302, 0.05230212180799747);
+ double latitude = -3.469446951953614E-18;
+ Vector3D closeReference = new Vector3D(5177991.74844521, 3726070.452427455, 90.88067547897226);
+ Vector3D intersection = ellipsoid.pointAtLatitude(p, d, latitude, closeReference);
+ GeodeticPoint gp = ellipsoid.transform(intersection, ellipsoid.getBodyFrame(), null);
+ Assert.assertEquals(latitude, gp.getLatitude(), 1.0e-10);
+ Assert.assertEquals(2866.297, gp.getAltitude(), 1.0e-3);
+ }
+
@Test
public void testPointAtLatitudeErrorQuadraticEquation() throws RuggedException, OrekitException {
@@ -241,9 +255,8 @@ public class ExtendedEllipsoidTest {
public void testPointAtLatitudeError() throws RuggedException, OrekitException {
Vector3D p = new Vector3D(-3052690.88784496, 6481300.309857268, 25258.7478104745);
-
- Vector3D d = new Vector3D(0.3063261631703422, -0.951393802931811, -0.0318451487715828);
- double latitude = 1.3959699281945737E-14;
+ Vector3D d = new Vector3D(0.6, -0.8, 0.0);
+ double latitude = 0.1;
Vector3D c = new Vector3D(-2809972.5765414005, 5727461.020250551, 26.163518446261833);
try {
diff --git a/src/test/resources/replay/replay-direct-loc-02.txt b/src/test/resources/replay/replay-direct-loc-02.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e79f13b9f2f044202f61dde120642515aef11a7d
--- /dev/null
+++ b/src/test/resources/replay/replay-direct-loc-02.txt
@@ -0,0 +1,28 @@
+# Rugged library dump file, created on 2015-04-23T15:12:38Z
+# all units are SI units (m, m/s, rad ...)
+direct location: date 2013-07-09T08:04:01.64555485306736Z position 0.000000000000000e+00 0.000000000000000e+00 0.000000000000000e+00 los -2.397859534323394e-02 1.069618325126766e-01 9.939739399066714e-01 lightTime false aberration false
+span: minDate 2013-07-09T07:44:31.00000000000000Z maxDate 2013-07-09T08:13:29.00000000000000Z tStep 1.000000000000000e-01 tolerance 1.000000000000000e+01 inertialFrame EME2000
+transform: index 11706 body r -9.126227111915787e-01 2.903948203646350e-04 6.031760832458322e-04 4.088023224821415e-01 Ω -9.759363848525831e-08 2.685469893129743e-09 -7.292108611062513e-05 ΩDot -2.894633518922737e-16 -1.401923338483166e-16 3.822089661987461e-19 spacecraft p -9.813172457506880e-01 -2.293861089274287e-01 -7.170114055550980e+06 v 1.721804626498136e-02 -5.160520845023520e-02 -8.541224850640113e+00 a -3.826063343921664e-01 -2.431259446863342e+00 6.594385725102986e-03 r -5.316321568749174e-01 5.693171838660510e-01 4.158849079021506e-01 -4.693452218895723e-01 Ω 1.024382530168276e-03 -9.422884774760976e-05 1.535223674917447e-04 ΩDot -1.147097938604457e-08 1.286556172365012e-07 3.416768201783130e-07
+algorithm: DUVENHAGE
+ellipsoid: ae 6.378137000000000e+06 f 3.352810664747481e-03 frame ITRF_CIO_CONV_2010_SIMPLE_EOP
+DEM tile: t0 latMin -1.745329251994330e-02 latStep 1.454441043328608e-05 latRows 1201 lonMin 6.108652381980153e-01 lonStep 1.454441043328608e-05 lonCols 1201
+DEM cell: t0 latIndex 939 lonIndex 2 elevation 1.141932549078131e+03
+DEM cell: t0 latIndex 635 lonIndex 1044 elevation 3.059853107443227e+03
+DEM cell: t0 latIndex 1184 lonIndex 865 elevation 2.283370902248552e+03
+DEM cell: t0 latIndex 1185 lonIndex 865 elevation 2.283369398664941e+03
+DEM cell: t0 latIndex 1184 lonIndex 866 elevation 2.275368031868235e+03
+DEM cell: t0 latIndex 1185 lonIndex 866 elevation 2.281366531059011e+03
+DEM tile: t1 latMin 0.000000000000000e+00 latStep 1.454441043328608e-05 latRows 1201 lonMin 6.108652381980153e-01 lonStep 1.454441043328608e-05 lonCols 1201
+DEM cell: t1 latIndex 1162 lonIndex 1200 elevation 8.242020012027025e+02
+DEM cell: t1 latIndex 1200 lonIndex 612 elevation 2.889312890594999e+03
+DEM cell: t1 latIndex 0 lonIndex 888 elevation 2.114929904457000e+03
+DEM cell: t1 latIndex 0 lonIndex 889 elevation 2.115931793474417e+03
+DEM cell: t1 latIndex 1 lonIndex 888 elevation 2.114929426260988e+03
+DEM cell: t1 latIndex 1 lonIndex 889 elevation 2.109931313132628e+03
+DEM cell: t1 latIndex 0 lonIndex 885 elevation 2.135924237404747e+03
+DEM cell: t1 latIndex 1 lonIndex 885 elevation 2.135923765646068e+03
+DEM cell: t1 latIndex 0 lonIndex 886 elevation 2.129926126422164e+03
+DEM cell: t1 latIndex 1 lonIndex 886 elevation 2.129925652517708e+03
+DEM cell: t1 latIndex 0 lonIndex 887 elevation 2.124928015439582e+03
+DEM cell: t1 latIndex 1 lonIndex 887 elevation 2.124927539389348e+03
+direct location result: latitude 6.217101136089454e-06 longitude 6.237751969228724e-01 elevation 2.118695205480773e+03