diff --git a/src/main/java/org/orekit/rugged/utils/ExtendedEllipsoid.java b/src/main/java/org/orekit/rugged/utils/ExtendedEllipsoid.java
index d44fc18ec635011a7074d854ba651c7cbac86d82..e36409d5ca3b04c3d6b769baf0a12a72f4bbd5a5 100644
--- a/src/main/java/org/orekit/rugged/utils/ExtendedEllipsoid.java
+++ b/src/main/java/org/orekit/rugged/utils/ExtendedEllipsoid.java
@@ -79,7 +79,8 @@ public class ExtendedEllipsoid extends OneAxisEllipsoid {
* @param los pixel line-of-sight, not necessarily normalized (in body frame)
* @param latitude latitude with respect to ellipsoid
* @param closeReference reference point used to select the closest solution
- * when there are two points at the desired latitude along the line
+ * when there are two points at the desired latitude along the line, it should
+ * be close to los surface intersection
* @return point at latitude
* @exception RuggedException if no such point exists
*/
@@ -100,22 +101,18 @@ public class ExtendedEllipsoid extends OneAxisEllipsoid {
// 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
+ // first searching kâ‚€ such that position + kâ‚€ los is close to closeReference, and
+ // then using position + kâ‚€ los as the new initial position, which should be in
+ // the neighborhood of the solution
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 double k0 = Vector3D.dotProduct(closeReference.subtract(position), los) / los.getNormSq();
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 a = MathArrays.linearCombination(+sinPhi2, los.getX() * los.getX() + los.getY() * los.getY(),
+ -cosPhi2, los.getZ() * los.getZ());
final double b = MathArrays.linearCombination(+sinPhi2, MathArrays.linearCombination(delta.getX(), los.getX(),
delta.getY(), los.getY()),
-cosPhi2, delta.getZ() * los.getZ());
@@ -123,15 +120,13 @@ public class ExtendedEllipsoid extends OneAxisEllipsoid {
-cosPhi2, delta.getZ() * delta.getZ());
// find the two intersections along the line
- final double bb = b * b;
- final double ac = a0 * c;
- if (bb < ac) {
+ if (b * b < a * c) {
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) / a0 : c / (s - b);
- final double k2 = c / (a0 * k1);
+ final double s = FastMath.sqrt(MathArrays.linearCombination(b, b, -a, c));
+ final double k1 = (b > 0) ? -(s + b) / a : c / (s - b);
+ final double k2 = c / (a * k1);
// the quadratic equation has two solutions
final boolean k1IsOK = (delta.getZ() + k1 * los.getZ()) * latitude >= 0;
diff --git a/src/site/xdoc/changes.xml b/src/site/xdoc/changes.xml
index 8f9d7f4595a5a8b1de8d019055f51b87e64934ee..dc2db34b2f98725bf7db99acbdf6acfa4320d368 100644
--- a/src/site/xdoc/changes.xml
+++ b/src/site/xdoc/changes.xml
@@ -22,6 +22,9 @@
<body>
<release version="1.0" date="TBD"
description="TBD">
+ <action dev="luc" type="fix" >
+ Improved latitude crossing robustness.
+ </action>
<action dev="luc" type="add" >
Added a CONSTANT_ELEVATION_OVER_ELLIPSOID algorithm, similar in spirit
to the IGNORE_DEM_USE_ELLIPSOID, but with a user-specified elevation
diff --git a/src/test/java/org/orekit/rugged/api/RuggedTest.java b/src/test/java/org/orekit/rugged/api/RuggedTest.java
index 00252890401605ca4dee585734ea44c9d7c237e5..69323887d95ae8dda34efc06c78006dc2f759d6b 100644
--- a/src/test/java/org/orekit/rugged/api/RuggedTest.java
+++ b/src/test/java/org/orekit/rugged/api/RuggedTest.java
@@ -656,7 +656,7 @@ public class RuggedTest {
public void testInverseLocation()
throws RuggedException, OrekitException, URISyntaxException {
checkInverseLocation(2000, false, false, 3.0e-7, 5.0e-6);
- checkInverseLocation(2000, false, true, 6.0e-6, 2.0e-7);
+ checkInverseLocation(2000, false, true, 9.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);
}
diff --git a/src/test/java/org/orekit/rugged/utils/ExtendedEllipsoidTest.java b/src/test/java/org/orekit/rugged/utils/ExtendedEllipsoidTest.java
index 806a809561324644eb3b7410f769d3d993613c6f..d3efa2534c8a63c089174b481c421cced463aa63 100644
--- a/src/test/java/org/orekit/rugged/utils/ExtendedEllipsoidTest.java
+++ b/src/test/java/org/orekit/rugged/utils/ExtendedEllipsoidTest.java
@@ -78,12 +78,11 @@ public class ExtendedEllipsoidTest {
Vector3D p = new Vector3D(3220103.0, 69623.0, -6449822.0);
Vector3D d = new Vector3D(1.0, 2.0, 3.0);
- double epsilon = 7.0e-8; // will be used only for the first point, as intersection is almost along a generatrix
+ double epsilon = 5.0e-15;
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(), epsilon);
- epsilon = 1.0e-15; // after first point, far from the generatrix, intersection is very accurate
}
}
@@ -131,14 +130,14 @@ public class ExtendedEllipsoidTest {
Vector3D d = new Vector3D(1.0, 2.0, 0.1);
double latitude = -0.5;
- Vector3D pPlus = ellipsoid.pointAtLatitude(p, d, latitude, new Vector3D(1, p, +1.0e10, d));
+ Vector3D pPlus = ellipsoid.pointAtLatitude(p, d, latitude, new Vector3D(1, p, +2.0e7, d));
GeodeticPoint gpPlus = ellipsoid.transform(pPlus, ellipsoid.getBodyFrame(), null);
- Assert.assertEquals(latitude, gpPlus.getLatitude(), 1.0e-12);
+ Assert.assertEquals(latitude, gpPlus.getLatitude(), 3.0e-16);
Assert.assertEquals(20646364.047, Vector3D.dotProduct(d, pPlus.subtract(p)), 0.001);
- Vector3D pMinus = ellipsoid.pointAtLatitude(p, d, latitude, new Vector3D(1, p, -1.0e10, d));
+ Vector3D pMinus = ellipsoid.pointAtLatitude(p, d, latitude, new Vector3D(1, p, -3.0e7, d));
GeodeticPoint gpMinus = ellipsoid.transform(pMinus, ellipsoid.getBodyFrame(), null);
- Assert.assertEquals(latitude, gpMinus.getLatitude(), 1.0e-12);
+ Assert.assertEquals(latitude, gpMinus.getLatitude(), 3.0e-16);
Assert.assertEquals(-31797895.234, Vector3D.dotProduct(d, pMinus.subtract(p)), 0.001);
}
@@ -269,6 +268,20 @@ public class ExtendedEllipsoidTest {
}
+ @Test
+ public void testPointAtLatitudeIssue1() throws RuggedException, OrekitException {
+
+ Vector3D position = new Vector3D(-1988136.619268088, -2905373.394638188, 6231185.484365295);
+ Vector3D los = new Vector3D(0.3489121277213534, 0.3447806500507106, -0.8714279261531437);
+ Vector3D close = new Vector3D(-1709383.0948608494, -2630206.8820586684, 5535282.169189105);
+ double latitude = 1.0581058590215624;
+
+ Vector3D s = ellipsoid.pointAtLatitude(position, los, latitude, close);
+ GeodeticPoint gp = ellipsoid.transform(s, ellipsoid.getBodyFrame(), null);
+ Assert.assertEquals(latitude, gp.getLatitude(), 1.0e-15);
+
+ }
+
@Before
public void setUp() {
try {