Merge branch 'pointAtLatitude'

Conflicts:
	src/test/java/org/orekit/rugged/api/RuggedTest.java

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;

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 protection against infinite loops in Duvenhage algorithm.
       </action>

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 {