diff --git a/rugged-core/src/main/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithm.java b/rugged-core/src/main/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithm.java
index acdf0fafb777e497fc0ba0bd0ea8386a0225e551..708da80b535f5ad90a4db446eddd9b74f9aff589 100644
--- a/rugged-core/src/main/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithm.java
+++ b/rugged-core/src/main/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithm.java
@@ -17,6 +17,7 @@
package org.orekit.rugged.core.duvenhage;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
+import org.apache.commons.math3.util.FastMath;
import org.orekit.bodies.GeodeticPoint;
import org.orekit.errors.OrekitException;
import org.orekit.rugged.api.RuggedException;
@@ -69,10 +70,11 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm {
MinMaxTreeTile tile = cache.getTile(gp0.getLatitude(), gp0.getLongitude());
GeodeticPoint current = null;
+ double hMax = tile.getMaxElevation();
while (current == null) {
// find where line-of-sight crosses tile max altitude
- final Vector3D entryP = ellipsoid.pointAtAltitude(position, los, tile.getMaxElevation() + STEP);
+ final Vector3D entryP = ellipsoid.pointAtAltitude(position, los, hMax + STEP);
if (Vector3D.dotProduct(entryP.subtract(position), los) < 0) {
// the entry point is behind spacecraft!
throw new RuggedException(RuggedMessages.DEM_ENTRY_POINT_IS_BEHIND_SPACECRAFT);
@@ -82,6 +84,7 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm {
if (tile.getLocation(current.getLatitude(), current.getLongitude()) != Tile.Location.IN_TILE) {
// the entry point is in another tile
tile = cache.getTile(current.getLatitude(), current.getLongitude());
+ hMax = FastMath.max(hMax, tile.getMaxElevation());
current = null;
}
@@ -151,22 +154,27 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm {
* @exception RuggedException if intersection cannot be found
* @exception OrekitException if points cannot be converted to geodetic coordinates
*/
- private GeodeticPoint recurseIntersection(final ExtendedEllipsoid ellipsoid, final Vector3D position,
- final Vector3D los, final MinMaxTreeTile tile,
- final GeodeticPoint entry, final int entryLat, final int entryLon,
- final GeodeticPoint exit, final int exitLat, final int exitLon)
+ private GeodeticPoint recurseIntersection(final ExtendedEllipsoid ellipsoid,
+ final Vector3D position, final Vector3D los,
+ final MinMaxTreeTile tile,
+ final GeodeticPoint entry,
+ final int entryLat, final int entryLon,
+ final GeodeticPoint exit,
+ final int exitLat, final int exitLon)
throws RuggedException, OrekitException {
if (entryLat == exitLat && entryLon == exitLon) {
// we have narrowed the search down to a single Digital Elevation Model pixel
- GeodeticPoint intersection = tile.pixelIntersection(entry, ellipsoid.convertLos(entry, los), exitLat, exitLon);
+ GeodeticPoint intersection = tile.pixelIntersection(entry, ellipsoid.convertLos(entry, los),
+ exitLat, exitLon);
if (intersection != null) {
// improve the point, by projecting it back on the 3D line, fixing the small body curvature at pixel level
final Vector3D delta = ellipsoid.transform(intersection).subtract(position);
final double s = Vector3D.dotProduct(delta, los) / los.getNormSq();
final GeodeticPoint projected = ellipsoid.transform(new Vector3D(1, position, s, los),
ellipsoid.getBodyFrame(), null);
- intersection = tile.pixelIntersection(projected, ellipsoid.convertLos(projected, los), exitLat, exitLon);
+ intersection = tile.pixelIntersection(projected, ellipsoid.convertLos(projected, los),
+ exitLat, exitLon);
}
return intersection;
}
@@ -192,27 +200,32 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm {
for (final int crossingLon : crossings) {
// compute segment endpoints
- final Vector3D crossingP = ellipsoid.pointAtLongitude(position, los,
- tile.getLongitudeAtIndex(crossingLon));
- final GeodeticPoint crossingGP = ellipsoid.transform(crossingP, ellipsoid.getBodyFrame(), null);
- final int crossingLat = tile.getLatitudeIndex(crossingGP.getLatitude());
-
- // adjust indices as the crossing point is by definition between the sub-tiles
- final int crossingLonBefore = crossingLon - (entryLon <= exitLon ? 1 : 0);
- final int crossingLonAfter = crossingLon - (entryLon <= exitLon ? 0 : 1);
-
- // look for intersection
- final GeodeticPoint intersection = recurseIntersection(ellipsoid, position, los, tile,
- previousGP, previousLat, previousLon,
- crossingGP, crossingLat, crossingLonBefore);
- if (intersection != null) {
- return intersection;
- }
+ final double longitude = tile.getLongitudeAtIndex(crossingLon);
+ if (longitude >= FastMath.min(entry.getLongitude(), exit.getLongitude()) &&
+ longitude <= FastMath.max(entry.getLongitude(), exit.getLongitude())) {
+
+ final Vector3D crossingP = ellipsoid.pointAtLongitude(position, los, longitude);
+ final GeodeticPoint crossingGP = ellipsoid.transform(crossingP, ellipsoid.getBodyFrame(), null);
+ final int crossingLat = tile.getLatitudeIndex(crossingGP.getLatitude());
+
+ // adjust indices as the crossing point is by definition between the sub-tiles
+ final int crossingLonBefore = crossingLon - (entryLon <= exitLon ? 1 : 0);
+ final int crossingLonAfter = crossingLon - (entryLon <= exitLon ? 0 : 1);
+
+ // look for intersection
+ final GeodeticPoint intersection = recurseIntersection(ellipsoid, position, los, tile,
+ previousGP, previousLat, previousLon,
+ crossingGP, crossingLat, crossingLonBefore);
+ if (intersection != null) {
+ return intersection;
+ }
+
+ // prepare next segment
+ previousGP = crossingGP;
+ previousLat = crossingLat;
+ previousLon = crossingLonAfter;
- // prepare next segment
- previousGP = crossingGP;
- previousLat = crossingLat;
- previousLon = crossingLonAfter;
+ }
}
} else {
@@ -221,27 +234,33 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm {
for (final int crossingLat : crossings) {
// compute segment endpoints
- final Vector3D crossingP = ellipsoid.pointAtLatitude(position, los,
- tile.getLatitudeAtIndex(crossingLat));
- final GeodeticPoint crossingGP = ellipsoid.transform(crossingP, ellipsoid.getBodyFrame(), null);
- final int crossingLon = tile.getLongitudeIndex(crossingGP.getLongitude());
-
- // adjust indices as the crossing point is by definition between the sub-tiles
- final int crossingLatBefore = crossingLat - (entryLat <= exitLat ? 1 : 0);
- final int crossingLatAfter = crossingLat - (entryLat <= exitLat ? 0 : 1);
-
- // look for intersection
- final GeodeticPoint intersection = recurseIntersection(ellipsoid, position, los, tile,
- previousGP, previousLat, previousLon,
- crossingGP, crossingLatBefore, crossingLon);
- if (intersection != null) {
- return intersection;
- }
+ final double latitude = tile.getLatitudeAtIndex(crossingLat);
+ if (latitude >= FastMath.min(entry.getLatitude(), exit.getLatitude()) &&
+ latitude <= FastMath.max(entry.getLatitude(), exit.getLatitude())) {
+
+ final Vector3D crossingP = ellipsoid.pointAtLatitude(position, los,
+ tile.getLatitudeAtIndex(crossingLat));
+ final GeodeticPoint crossingGP = ellipsoid.transform(crossingP, ellipsoid.getBodyFrame(), null);
+ final int crossingLon = tile.getLongitudeIndex(crossingGP.getLongitude());
+
+ // adjust indices as the crossing point is by definition between the sub-tiles
+ final int crossingLatBefore = crossingLat - (entryLat <= exitLat ? 1 : 0);
+ final int crossingLatAfter = crossingLat - (entryLat <= exitLat ? 0 : 1);
+
+ // look for intersection
+ final GeodeticPoint intersection = recurseIntersection(ellipsoid, position, los, tile,
+ previousGP, previousLat, previousLon,
+ crossingGP, crossingLatBefore, crossingLon);
+ if (intersection != null) {
+ return intersection;
+ }
+
+ // prepare next segment
+ previousGP = crossingGP;
+ previousLat = crossingLatAfter;
+ previousLon = crossingLon;
- // prepare next segment
- previousGP = crossingGP;
- previousLat = crossingLatAfter;
- previousLon = crossingLon;
+ }
}
}
diff --git a/rugged-core/src/test/java/org/orekit/rugged/core/AbstractAlgorithmTest.java b/rugged-core/src/test/java/org/orekit/rugged/core/AbstractAlgorithmTest.java
index d940603bb877d9ac8d4498d5d84c7edc6037ffd1..6605d82be2df0a435cee5023f2ae9041e5a3ca59 100644
--- a/rugged-core/src/test/java/org/orekit/rugged/core/AbstractAlgorithmTest.java
+++ b/rugged-core/src/test/java/org/orekit/rugged/core/AbstractAlgorithmTest.java
@@ -20,6 +20,7 @@ package org.orekit.rugged.core;
import java.io.File;
import java.net.URISyntaxException;
+import org.apache.commons.math3.geometry.euclidean.threed.Line;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math3.util.FastMath;
@@ -86,6 +87,7 @@ public abstract class AbstractAlgorithmTest {
Vector3D position = state.getPVCoordinates(earth.getBodyFrame()).getPosition();
Vector3D los = groundP.subtract(position);
GeodeticPoint result = algorithm.intersection(earth, position, los);
+ checkIntersection(position, los, result);
Assert.assertEquals(0.0, groundP.distance(earth.transform(result)), 2.0e-9);
}
@@ -111,6 +113,7 @@ public abstract class AbstractAlgorithmTest {
Vector3D position = state.getPVCoordinates(earth.getBodyFrame()).getPosition();
Vector3D los = groundP.subtract(position);
GeodeticPoint result = algorithm.intersection(earth, position, los);
+ checkIntersection(position, los, result);
Assert.assertEquals(0.0, groundP.distance(earth.transform(result)), 2.0e-9);
}
@@ -145,10 +148,26 @@ public abstract class AbstractAlgorithmTest {
Vector3D position = state.getPVCoordinates(earth.getBodyFrame()).getPosition();
Vector3D los = groundP.subtract(position);
GeodeticPoint result = algorithm.intersection(earth, position, los);
+ checkIntersection(position, los, result);
Assert.assertEquals(0.0, groundP.distance(earth.transform(result)), 2.0e-9);
}
+ protected void checkIntersection(Vector3D position, Vector3D los, GeodeticPoint intersection)
+ throws RuggedException {
+
+ // check the point is on the line
+ Line line = new Line(position, new Vector3D(1, position, 1e6, los), 1.0e-12);
+ Assert.assertEquals(0.0, line.distance(earth.transform(intersection)), 3.0e-9);
+
+ // check the point is on the Digital Elevation Model
+ MinMaxTreeTile tile = new MinMaxTreeTileFactory().createTile();
+ updater.updateTile(intersection.getLatitude(), intersection.getLongitude(), tile);
+ Assert.assertEquals(tile.interpolateElevation(intersection.getLatitude(), intersection.getLongitude()),
+ intersection.getAltitude(), 2.0e-9);
+
+ }
+
protected void setUpMayonVolcanoContext()
throws RuggedException, OrekitException {
diff --git a/rugged-core/src/test/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithmTest.java b/rugged-core/src/test/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithmTest.java
index 40ed68fa9481b80e8c021d85015c08ef5f58d269..e8307b6828c367ceae693693b90ef4f0dd6df29d 100644
--- a/rugged-core/src/test/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithmTest.java
+++ b/rugged-core/src/test/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithmTest.java
@@ -18,7 +18,6 @@ package org.orekit.rugged.core.duvenhage;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
-import org.junit.Assert;
import org.junit.Test;
import org.orekit.bodies.GeodeticPoint;
import org.orekit.errors.OrekitException;
@@ -37,14 +36,21 @@ public class DuvenhageAlgorithmTest extends AbstractAlgorithmTest {
setUpMayonVolcanoContext();
final IntersectionAlgorithm algorithm = createAlgorithm();
algorithm.setUpTilesManagement(updater, 8);
- GeodeticPoint intersection = algorithm.intersection(earth,
- new Vector3D(-3787079.6453602533,
- 5856784.405679551,
- 1655869.0582939098),
- new Vector3D( 0.5127552821932051,
- -0.8254313129088879,
- -0.2361041470463311));
- Assert.assertEquals(16.0, intersection.getAltitude(), 1.0e-10);
+ Vector3D position = new Vector3D(-3787079.6453602533, 5856784.405679551, 1655869.0582939098);
+ Vector3D los = new Vector3D( 0.5127552821932051, -0.8254313129088879, -0.2361041470463311);
+ GeodeticPoint intersection = algorithm.intersection(earth, position, los);
+ checkIntersection(position, los, intersection);
+ }
+
+ @Test
+ public void testCrossingBeforeLineSegmentStart() throws RuggedException, OrekitException {
+ setUpMayonVolcanoContext();
+ final IntersectionAlgorithm algorithm = createAlgorithm();
+ algorithm.setUpTilesManagement(updater, 8);
+ Vector3D position = new Vector3D(-3787079.6453602533, 5856784.405679551, 1655869.0582939098);
+ Vector3D los = new Vector3D( 0.42804005978915904, -0.8670291034054828, -0.2550338037664377);
+ GeodeticPoint intersection = algorithm.intersection(earth, position, los);
+ checkIntersection(position, los, intersection);
}
}