diff --git a/src/main/java/org/orekit/rugged/core/ExtendedEllipsoid.java b/src/main/java/org/orekit/rugged/core/ExtendedEllipsoid.java
index e092e05acdbba37b2e0e774c4a2e7d00bc0c4604..b64fa130e7be9dab5c21a429ea1d9b4fa811a7bb 100644
--- a/src/main/java/org/orekit/rugged/core/ExtendedEllipsoid.java
+++ b/src/main/java/org/orekit/rugged/core/ExtendedEllipsoid.java
@@ -60,10 +60,13 @@ public class ExtendedEllipsoid extends OneAxisEllipsoid {
* @param position pixel position (in body frame)
* @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
* @return point at altitude
* @exception RuggedException if no such point exists
*/
- public Vector3D pointAtLatitude(final Vector3D position, final Vector3D los, final double latitude)
+ public Vector3D pointAtLatitude(final Vector3D position, final Vector3D los,
+ final double latitude, final Vector3D closeReference)
throws RuggedException {
// find apex of iso-latitude cone, somewhere along polar axis
@@ -93,15 +96,17 @@ public class ExtendedEllipsoid extends OneAxisEllipsoid {
final double k1 = (b > 0) ? -(s + b) / a : c / (s - b);
final double k2 = c / (a * k1);
- // the quadratic equation may find spurious solutions in the wrong cone nappe
+ // the quadratic equation has two solutions
final boolean k1IsOK = (delta.getZ() + k1 * los.getZ()) * latitude >= 0;
final boolean k2IsOK = (delta.getZ() + k2 * los.getZ()) * latitude >= 0;
final double selectedK;
if (k1IsOK) {
if (k2IsOK) {
// both solutions are in the good nappe,
- // we arbitrarily select the one closest to the initial position
- selectedK = FastMath.abs(k1) <= FastMath.abs(k2) ? k1 : k2;
+ // select the one closest to the specified reference
+ final double kRef = Vector3D.dotProduct(los, closeReference.subtract(position)) /
+ los.getNormSq();
+ selectedK = FastMath.abs(k1 - kRef) <= FastMath.abs(k2 - kRef) ? k1 : k2;
} else {
// only k1 is in the good nappe
selectedK = k1;
diff --git a/src/main/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithm.java b/src/main/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithm.java
index ea9cf9c2f1eda044eba7fe4632c043052b11a13e..ba364b25d0451fcabc71c4b295a5740677a4d67d 100644
--- a/src/main/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithm.java
+++ b/src/main/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithm.java
@@ -276,7 +276,8 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm {
} else {
// full computation of crossing point
final Vector3D crossingP = ellipsoid.pointAtLatitude(position, los,
- tile.getLatitudeAtIndex(crossingLat));
+ tile.getLatitudeAtIndex(crossingLat),
+ ellipsoid.transform(entry));
crossingGP = ellipsoid.transform(crossingP, ellipsoid.getBodyFrame(), null);
}
final int crossingLon = tile.getLongitudeIndex(crossingGP.getLongitude());
@@ -346,7 +347,7 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm {
switch (tile.getLocation(exitGP.getLatitude(), exitGP.getLongitude())) {
case SOUTH_WEST :
return new LimitPoint(ellipsoid,
- selectClosest(ellipsoid.pointAtLatitude(position, los, tile.getMinimumLatitude()),
+ selectClosest(ellipsoid.pointAtLatitude(position, los, tile.getMinimumLatitude(), exitP),
ellipsoid.pointAtLongitude(position, los, tile.getMinimumLongitude()),
position),
true);
@@ -356,17 +357,17 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm {
true);
case NORTH_WEST:
return new LimitPoint(ellipsoid,
- selectClosest(ellipsoid.pointAtLatitude(position, los, tile.getMaximumLatitude()),
+ selectClosest(ellipsoid.pointAtLatitude(position, los, tile.getMaximumLatitude(), exitP),
ellipsoid.pointAtLongitude(position, los, tile.getMinimumLongitude()),
position),
true);
case NORTH :
return new LimitPoint(ellipsoid,
- ellipsoid.pointAtLatitude(position, los, tile.getMaximumLatitude()),
+ ellipsoid.pointAtLatitude(position, los, tile.getMaximumLatitude(), exitP),
true);
case NORTH_EAST :
return new LimitPoint(ellipsoid,
- selectClosest(ellipsoid.pointAtLatitude(position, los, tile.getMaximumLatitude()),
+ selectClosest(ellipsoid.pointAtLatitude(position, los, tile.getMaximumLatitude(), exitP),
ellipsoid.pointAtLongitude(position, los, tile.getMaximumLongitude()),
position),
true);
@@ -376,13 +377,13 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm {
true);
case SOUTH_EAST :
return new LimitPoint(ellipsoid,
- selectClosest(ellipsoid.pointAtLatitude(position, los, tile.getMinimumLatitude()),
+ selectClosest(ellipsoid.pointAtLatitude(position, los, tile.getMinimumLatitude(), exitP),
ellipsoid.pointAtLongitude(position, los, tile.getMaximumLongitude()),
position),
true);
case SOUTH :
return new LimitPoint(ellipsoid,
- ellipsoid.pointAtLatitude(position, los, tile.getMinimumLatitude()),
+ ellipsoid.pointAtLatitude(position, los, tile.getMinimumLatitude(), exitP),
true);
case IN_TILE :
return new LimitPoint(exitGP, false);
diff --git a/src/test/java/org/orekit/rugged/core/ExtendedEllipsoidTest.java b/src/test/java/org/orekit/rugged/core/ExtendedEllipsoidTest.java
index f1d632a0f21b06db83fb876c0ffaf0dc6a6c618c..a8fe639d7e0d7b1071ccc30a43318e72dc9742d3 100644
--- a/src/test/java/org/orekit/rugged/core/ExtendedEllipsoidTest.java
+++ b/src/test/java/org/orekit/rugged/core/ExtendedEllipsoidTest.java
@@ -79,7 +79,7 @@ public class ExtendedEllipsoidTest {
Vector3D d = new Vector3D(1.0, 2.0, 3.0);
for (double latitude = -d.getDelta() + 1.0e-6; latitude < d.getDelta(); latitude += 0.1) {
- GeodeticPoint gp = ellipsoid.transform(ellipsoid.pointAtLatitude(p, d, latitude),
+ GeodeticPoint gp = ellipsoid.transform(ellipsoid.pointAtLatitude(p, d, latitude, p),
ellipsoid.getBodyFrame(), null);
Assert.assertEquals(latitude, gp.getLatitude(), 1.0e-12);
}
@@ -87,19 +87,57 @@ public class ExtendedEllipsoidTest {
}
@Test
- public void testPointAtLatitudeTwoPointsInGoodNappe() throws RuggedException, OrekitException {
+ public void testPointAtLatitudeTwoPointsSameSide() throws RuggedException, OrekitException {
+
+ // the line of sight is almost parallel an iso-latitude cone generatrix
+ // the spacecraft is at latitude lTarget - 0.951", and altitude 794.6km
+ // so at a latitude slightly less than the target
+ // the line of sight crosses the latitude cone first about 70km along line of sight
+ // (so still at a very high altitude) and a second time about 798km along line of sight,
+ // only a few hundreds meters above allipsoid
+ // Note that this happens despite the line of sight is not along nadir, the longitudes
+ // of the spacecraft and crossing points span in a 0.88° wide longitude range
+ Vector3D position = new Vector3D(-748528.2769999998, -5451658.432000002, 4587158.354);
+ Vector3D los = new Vector3D(0.010713435156834539, 0.7688536080293823, -0.6393350856376809);
+ double h = ellipsoid.transform(position, ellipsoid.getBodyFrame(), null).getAltitude();
+ double lTarget = 0.6978408125890662;
+
+ // spacecraft is in LEO
+ Assert.assertEquals(h, 794652.782, 0.001);
+
+ Vector3D pHigh = ellipsoid.pointAtLatitude(position, los, lTarget, position);
+ GeodeticPoint gpHigh = ellipsoid.transform(pHigh, ellipsoid.getBodyFrame(), null);
+ Assert.assertEquals(lTarget, gpHigh.getLatitude(), 1.0e-12);
+ // first crossing point is high, but below spacecraft and along positive line of sight
+ Assert.assertEquals(724335.409, gpHigh.getAltitude(), 0.001);
+ Assert.assertTrue(Vector3D.dotProduct(pHigh.subtract(position), los) > 0);
+
+ Vector3D pLow = ellipsoid.pointAtLatitude(position, los, lTarget,
+ new Vector3D(1, position, 900000, los));
+ GeodeticPoint gpLow = ellipsoid.transform(pLow, ellipsoid.getBodyFrame(), null);
+ Assert.assertEquals(lTarget, gpLow.getLatitude(), 1.0e-12);
+ // second crossing point is almost on ground, also along positive line of sight
+ Assert.assertEquals(492.804, gpLow.getAltitude(), 0.001);
+ Assert.assertTrue(Vector3D.dotProduct(pLow.subtract(position), los) > 0);
+
+ }
+
+ @Test
+ public void testPointAtLatitudeTwoPointsOppositeSides() throws RuggedException, OrekitException {
Vector3D p = new Vector3D(3220103.0, 69623.0, -6449822.0);
Vector3D d = new Vector3D(1.0, 2.0, 0.1);
double latitude = -0.5;
- GeodeticPoint gpPlus = ellipsoid.transform(ellipsoid.pointAtLatitude(p, d, latitude),
- ellipsoid.getBodyFrame(), null);
+ Vector3D pPlus = ellipsoid.pointAtLatitude(p, d, latitude, new Vector3D(1, p, +1.0e10, d));
+ GeodeticPoint gpPlus = ellipsoid.transform(pPlus, ellipsoid.getBodyFrame(), null);
Assert.assertEquals(latitude, gpPlus.getLatitude(), 1.0e-12);
+ Assert.assertEquals(20646364.047, Vector3D.dotProduct(d, pPlus.subtract(p)), 0.001);
- GeodeticPoint gpMinus = ellipsoid.transform(ellipsoid.pointAtLatitude(p, d.negate(), latitude),
- ellipsoid.getBodyFrame(), null);
+ Vector3D pMinus = ellipsoid.pointAtLatitude(p, d, latitude, new Vector3D(1, p, -1.0e10, d));
+ GeodeticPoint gpMinus = ellipsoid.transform(pMinus, ellipsoid.getBodyFrame(), null);
Assert.assertEquals(latitude, gpMinus.getLatitude(), 1.0e-12);
+ Assert.assertEquals(-31797895.234, Vector3D.dotProduct(d, pMinus.subtract(p)), 0.001);
}
@@ -111,7 +149,7 @@ public class ExtendedEllipsoidTest {
double latitude = -1.4;
try {
- ellipsoid.pointAtLatitude(p, d, latitude);
+ ellipsoid.pointAtLatitude(p, d, latitude, p);
Assert.fail("an error should have been triggered");
} catch (RuggedException re) {
Assert.assertEquals(RuggedMessages.LINE_OF_SIGHT_NEVER_CROSSES_LATITUDE, re.getSpecifier());
@@ -128,7 +166,7 @@ public class ExtendedEllipsoidTest {
double latitude = 0.5;
try {
- ellipsoid.pointAtLatitude(p, d, latitude);
+ ellipsoid.pointAtLatitude(p, d, latitude, p);
Assert.fail("an error should have been triggered");
} catch (RuggedException re) {
Assert.assertEquals(RuggedMessages.LINE_OF_SIGHT_NEVER_CROSSES_LATITUDE, re.getSpecifier());