From 1db62b0129f325028948db1684eb6ca4fed92424 Mon Sep 17 00:00:00 2001
From: Luc Maisonobe <luc@orekit.org>
Date: Wed, 19 Mar 2014 15:18:03 +0100
Subject: [PATCH] Implemented intersection at pixel level.
---
.../rugged/core/BasicScanAlgorithm.java | 2 +-
.../core/duvenhage/DuvenhageAlgorithm.java | 2 +-
.../orekit/rugged/core/raster/SimpleTile.java | 92 ++++++++++++++-
.../org/orekit/rugged/core/raster/Tile.java | 17 ++-
.../rugged/core/raster/SimpleTileTest.java | 110 ++++++++++++++++++
5 files changed, 218 insertions(+), 5 deletions(-)
diff --git a/rugged-core/src/main/java/org/orekit/rugged/core/BasicScanAlgorithm.java b/rugged-core/src/main/java/org/orekit/rugged/core/BasicScanAlgorithm.java
index a1ff5753..a2cfd3a3 100644
--- a/rugged-core/src/main/java/org/orekit/rugged/core/BasicScanAlgorithm.java
+++ b/rugged-core/src/main/java/org/orekit/rugged/core/BasicScanAlgorithm.java
@@ -118,7 +118,7 @@ public class BasicScanAlgorithm implements IntersectionAlgorithm {
for (final SimpleTile tile : scannedTiles) {
for (int i = latitudeIndex(tile, minLatitude); i < latitudeIndex(tile, maxLatitude); ++i) {
for (int j = longitudeIndex(tile, minLongitude); j < longitudeIndex(tile, maxLongitude); ++j) {
- GeodeticPoint gp = tile.pixelIntersection(ellipsoid, entryPoint, exitPoint, i, j);
+ GeodeticPoint gp = tile.pixelIntersection(entryPoint, exitPoint, i, j);
if (gp != null) {
final Vector3D point = ellipsoid.transform(intersectionGP);
final double dot = Vector3D.dotProduct(point.subtract(position), los);
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 40f30782..aebeb54e 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
@@ -112,7 +112,7 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm {
// we have narrowed the search down to a single Digital Elevation Model pixel
final GeodeticPoint intersection =
- tile.pixelIntersection(ellipsoid, current, next, nextLatIndex, nextLonIndex);
+ tile.pixelIntersection(current, next, nextLatIndex, nextLonIndex);
if (intersection != null) {
return intersection;
} else {
diff --git a/rugged-core/src/main/java/org/orekit/rugged/core/raster/SimpleTile.java b/rugged-core/src/main/java/org/orekit/rugged/core/raster/SimpleTile.java
index 9778429c..8369ee2c 100644
--- a/rugged-core/src/main/java/org/orekit/rugged/core/raster/SimpleTile.java
+++ b/rugged-core/src/main/java/org/orekit/rugged/core/raster/SimpleTile.java
@@ -17,6 +17,7 @@
package org.orekit.rugged.core.raster;
import org.apache.commons.math3.util.FastMath;
+import org.orekit.bodies.GeodeticPoint;
import org.orekit.rugged.api.RuggedException;
import org.orekit.rugged.api.RuggedMessages;
@@ -240,6 +241,93 @@ public class SimpleTile implements Tile {
}
+ /** {@inheritDoc} */
+ @Override
+ public GeodeticPoint pixelIntersection(final GeodeticPoint pA, final GeodeticPoint pB,
+ final int latitudeIndex, final int longitudeIndex)
+ throws RuggedException {
+
+ // Digital Elevation Mode coordinates at pixel vertices
+ final double x00 = getLongitudeAtIndex(longitudeIndex);
+ final double y00 = getLatitudeAtIndex(latitudeIndex);
+ final double z00 = getElevationAtIndices(latitudeIndex, longitudeIndex);
+ final double z01 = getElevationAtIndices(latitudeIndex + 1, longitudeIndex);
+ final double z10 = getElevationAtIndices(latitudeIndex, longitudeIndex + 1);
+ final double z11 = getElevationAtIndices(latitudeIndex + 1, longitudeIndex + 1);
+
+ // line-of-sight coordinates at close points
+ final double dxA = (pA.getLongitude() - x00) / longitudeStep;
+ final double dyA = (pA.getLatitude() - y00) / latitudeStep;
+ final double dzA = pA.getAltitude();
+ final double dxB = (pB.getLongitude() - x00) / longitudeStep;
+ final double dyB = (pB.getLatitude() - y00) / latitudeStep;
+ final double dzB = pB.getAltitude();
+
+ // points along line-of-sight can be defined as a linear combination
+ // between pA and pB depending on free variable t: p(t) = pA * (1 - t) + pB * t.
+ // As the point latitude and longitude are linear with respect to t,
+ // and as Digital Elevation Model is quadratic with respect to latitude
+ // and longitude, the altitude of DEM at same horizontal position as
+ // point is quadratic in t:
+ // z_DEM(t) = u t² + v t + w
+ final double u = (dxA - dxB) * (dyA - dyB) * (z00 - z10 - z01 + z11);
+ final double v = ((dxA - dxB) * (1 - dyA) + (dyA - dyB) * (1 - dxA)) * z00 +
+ (dxA * (dyA - dyB) - (dxA - dxB) * (1 - dyA)) * z10 +
+ (dyA * (dxA - dxB) - (dyA - dyB) * (1 - dxA)) * z01 +
+ ((dxB - dxA) * dyA + (dyB - dyA) * dxA) * z11;
+ final double w = (1 - dxA) * ((1 - dyA) * z00 + dyA * z01) +
+ dxA * ((1 - dyA) * z10 + dyA * z11);
+
+ // subtract linear z from line-of-sight
+ // z_DEM(t) - z_LOS(t) = a t² + b t + c
+ final double a = u;
+ final double b = v + dzA - dzB;
+ final double c = w - dzA;
+
+ // solve the quadratic equation
+ final double b2 = b * b;
+ final double fac = 4 * a * c;
+ if (b2 < fac) {
+ // no intersection at all
+ return null;
+ }
+ final double s = FastMath.sqrt(b2 - fac);
+ final double t1 = (b < 0) ? (s - b) / (2 * a) : -2 * c / (b + s);
+ final double t2 = c / (a * t1);
+
+ final double dx1 = dxA * (1 - t1) + dxB * t1;
+ final double dy1 = dyA * (1 - t1) + dyB * t1;
+ final GeodeticPoint p1;
+ if (dx1 >= 0 && dx1 <= 1 && dy1 >= 0 && dy1 <= 1) {
+ p1 = new GeodeticPoint(pA.getLatitude() * (1 - t1) + pB.getLatitude() * t1,
+ pA.getLongitude() * (1 - t1) + pB.getLongitude() * t1,
+ pA.getAltitude() * (1 - t1) + pB.getAltitude() * t1);
+ } else {
+ p1 = null;
+ }
+
+ final double dx2 = dxA * (1 - t2) + dxB * t2;
+ final double dy2 = dyA * (1 - t2) + dyB * t2;
+ final GeodeticPoint p2;
+ if (dx2 >= 0 && dx2 <= 1 && dy2 >= 0 && dy2 <= 1) {
+ p2 = new GeodeticPoint(pA.getLatitude() * (1 - t2) + pB.getLatitude() * t2,
+ pA.getLongitude() * (1 - t2) + pB.getLongitude() * t2,
+ pA.getAltitude() * (1 - t2) + pB.getAltitude() * t2);
+ } else {
+ p2 = null;
+ }
+
+ // select the first point along line-of-sight
+ if (p1 == null) {
+ return p2;
+ } else if (p2 == null) {
+ return p1;
+ } else {
+ return t1 <= t2 ? p1 : p2;
+ }
+
+ }
+
/** {@inheritDoc} */
@Override
public int getLatitudeIndex(double latitude) {
@@ -248,7 +336,7 @@ public class SimpleTile implements Tile {
/** {@inheritDoc} */
@Override
- public int getLontitudeIndex(double longitude) {
+ public int getLongitudeIndex(double longitude) {
return (int) FastMath.floor(getDoubleLontitudeIndex(longitude));
}
@@ -272,7 +360,7 @@ public class SimpleTile implements Tile {
@Override
public Location getLocation(final double latitude, final double longitude) {
final int latitudeIndex = getLatitudeIndex(latitude);
- final int longitudeIndex = getLontitudeIndex(longitude);
+ final int longitudeIndex = getLongitudeIndex(longitude);
if (longitudeIndex < 0) {
if (latitudeIndex < 0) {
return Location.SOUTH_WEST;
diff --git a/rugged-core/src/main/java/org/orekit/rugged/core/raster/Tile.java b/rugged-core/src/main/java/org/orekit/rugged/core/raster/Tile.java
index 199acf48..6a254e15 100644
--- a/rugged-core/src/main/java/org/orekit/rugged/core/raster/Tile.java
+++ b/rugged-core/src/main/java/org/orekit/rugged/core/raster/Tile.java
@@ -16,6 +16,7 @@
*/
package org.orekit.rugged.core.raster;
+import org.orekit.bodies.GeodeticPoint;
import org.orekit.rugged.api.RuggedException;
import org.orekit.rugged.api.UpdatableTile;
@@ -105,7 +106,7 @@ public interface Tile extends UpdatableTile {
* @param longitude geodetic latitude
* @return longitude index (it may lie outside of the tile!)
*/
- int getLontitudeIndex(double longitude);
+ int getLongitudeIndex(double longitude);
/** Get the minimum elevation in the tile.
* @return minimum elevation in the tile
@@ -143,6 +144,20 @@ public interface Tile extends UpdatableTile {
double interpolateElevation(double latitude, double longitude)
throws RuggedException;
+ /** Find the intersection of a line-of-sight and a Digital Elevation Model pixel.
+ * @param pA first point on the line (closer to satellite)
+ * @param pB second point on the line (closer to ground)
+ * @param latitudeIndex latitude index of the Digital Elevation Model pixel
+ * @param longitudeIndex longitude index of the Digital Elevation Model pixel
+ * @return point corresponding to line-of-sight crossing the Digital Elevation Model surface
+ * if it lies within the pixel, null otherwise
+ * @exception RuggedException if intersection point cannot be computed
+ * @exception OrekitException if intersection point cannot be converted to geodetic coordinates
+ */
+ GeodeticPoint pixelIntersection(GeodeticPoint pA, GeodeticPoint pB,
+ int latitudeIndex, int longitudeIndex)
+ throws RuggedException;
+
/** Check if a tile covers a ground point.
* @param latitude ground point latitude
* @param longitude ground point longitude
diff --git a/rugged-core/src/test/java/org/orekit/rugged/core/raster/SimpleTileTest.java b/rugged-core/src/test/java/org/orekit/rugged/core/raster/SimpleTileTest.java
index 61e05f41..363d39d2 100644
--- a/rugged-core/src/test/java/org/orekit/rugged/core/raster/SimpleTileTest.java
+++ b/rugged-core/src/test/java/org/orekit/rugged/core/raster/SimpleTileTest.java
@@ -19,6 +19,7 @@ package org.orekit.rugged.core.raster;
import org.apache.commons.math3.util.FastMath;
import org.junit.Assert;
import org.junit.Test;
+import org.orekit.bodies.GeodeticPoint;
import org.orekit.rugged.api.RuggedException;
import org.orekit.rugged.api.RuggedMessages;
import org.orekit.rugged.core.raster.SimpleTile;
@@ -37,6 +38,8 @@ public class SimpleTileTest {
Assert.assertEquals(0, tile.getLongitudeStep(), 1.0e-10);
Assert.assertEquals(0, tile.getLatitudeRows());
Assert.assertEquals(0, tile.getLongitudeColumns());
+ Assert.assertEquals(0, tile.getMinElevation(), 1.0e-10);
+ Assert.assertEquals(0, tile.getMaxElevation(), 1.0e-10);
}
@Test
@@ -70,6 +73,7 @@ public class SimpleTileTest {
tile.setElevation(i, j, 1000 * i + j);
}
}
+ tile.tileUpdateCompleted();
Assert.assertEquals(1.0, tile.getMinimumLatitude(), 1.0e-10);
Assert.assertEquals(2.0, tile.getMinimumLongitude(), 1.0e-10);
@@ -77,6 +81,8 @@ public class SimpleTileTest {
Assert.assertEquals(0.2, tile.getLongitudeStep(), 1.0e-10);
Assert.assertEquals(100, tile.getLatitudeRows());
Assert.assertEquals(200, tile.getLongitudeColumns());
+ Assert.assertEquals(0.0, tile.getMinElevation(), 1.0e-10);
+ Assert.assertEquals(99199.0, tile.getMaxElevation(), 1.0e-10);
Assert.assertEquals(Location.SOUTH_WEST, tile.getLocation( 0.0, 1.0));
Assert.assertEquals(Location.WEST, tile.getLocation( 6.0, 1.0));
@@ -101,6 +107,7 @@ public class SimpleTileTest {
SimpleTile tile = new SimpleTileFactory().createTile();
tile.setGeometry(1.0, 2.0, 0.1, 0.2, 100, 200);
tile.setElevation(50, 100, 1000.0);
+ tile.tileUpdateCompleted();
checkOutOfBound( -1, 100, tile);
checkOutOfBound(100, 100, tile);
checkOutOfBound( 50, -1, tile);
@@ -127,6 +134,7 @@ public class SimpleTileTest {
tile.setElevation(20, 15, 210.0);
tile.setElevation(21, 14, 162.0);
tile.setElevation(21, 15, 95.0);
+ tile.tileUpdateCompleted();
Assert.assertEquals(150.5, tile.interpolateElevation(20.0, 14.5), 1.0e-10);
Assert.assertEquals(128.5, tile.interpolateElevation(21.0, 14.5), 1.0e-10);
Assert.assertEquals(146.1, tile.interpolateElevation(20.2, 14.5), 1.0e-10);
@@ -140,6 +148,7 @@ public class SimpleTileTest {
tile.setElevation(0, 1, 210.0);
tile.setElevation(1, 0, 162.0);
tile.setElevation(1, 1, 95.0);
+ tile.tileUpdateCompleted();
// the following points are 1/16 pixel out of tile
Assert.assertEquals(151.875, tile.interpolateElevation(-0.0625, 0.5), 1.0e-10);
Assert.assertEquals(127.125, tile.interpolateElevation( 1.0625, 0.5), 1.0e-10);
@@ -155,6 +164,7 @@ public class SimpleTileTest {
tile.setElevation(0, 1, 210.0);
tile.setElevation(1, 0, 162.0);
tile.setElevation(1, 1, 95.0);
+ tile.tileUpdateCompleted();
// the following points are 3/16 pixel out of tile
checkOutOfBound(-0.1875, 0.5, tile);
checkOutOfBound( 1.1875, 0.5, tile);
@@ -162,6 +172,85 @@ public class SimpleTileTest {
checkOutOfBound( 0.5, 1.1875, tile);
}
+ @Test
+ public void testPixelIntersection() throws RuggedException {
+ SimpleTile tile = new SimpleTileFactory().createTile();
+ tile.setGeometry(0.0, 0.0, 0.025, 0.025, 50, 50);
+ tile.setElevation(20, 14, 91.0);
+ tile.setElevation(20, 15, 210.0);
+ tile.setElevation(21, 14, 162.0);
+ tile.setElevation(21, 15, 95.0);
+ tile.tileUpdateCompleted();
+ GeodeticPoint gpA = new GeodeticPoint(tile.getLatitudeAtIndex(20) + 0.1 * tile.getLatitudeStep(),
+ tile.getLongitudeAtIndex(14) + 0.2 * tile.getLongitudeStep(),
+ 300.0);
+ GeodeticPoint gpB = new GeodeticPoint(tile.getLatitudeAtIndex(20) + 0.7 * tile.getLatitudeStep(),
+ tile.getLongitudeAtIndex(14) + 0.9 * tile.getLongitudeStep(),
+ 10.0);
+ GeodeticPoint gpIAB = tile.pixelIntersection(gpA, gpB, 20, 14);
+ checkInLine(gpA, gpB, gpIAB);
+ checkOnTile(tile, gpIAB);
+ GeodeticPoint gpIBA = tile.pixelIntersection(gpB, gpA, 20, 14);
+ checkInLine(gpA, gpB, gpIBA);
+ checkOnTile(tile, gpIBA);
+
+ Assert.assertEquals(gpIAB.getLatitude(), gpIBA.getLatitude(), 1.0e-10);
+ Assert.assertEquals(gpIAB.getLongitude(), gpIBA.getLongitude(), 1.0e-10);
+ Assert.assertEquals(gpIAB.getAltitude(), gpIBA.getAltitude(), 1.0e-10);
+
+ }
+
+ @Test
+ public void testPixelIntersection2Solutions() throws RuggedException {
+ SimpleTile tile = new SimpleTileFactory().createTile();
+ tile.setGeometry(0.0, 0.0, 0.025, 0.025, 50, 50);
+ tile.setElevation(20, 14, 91.0);
+ tile.setElevation(20, 15, 210.0);
+ tile.setElevation(21, 14, 162.0);
+ tile.setElevation(21, 15, 95.0);
+ tile.tileUpdateCompleted();
+ GeodeticPoint gpA = new GeodeticPoint(tile.getLatitudeAtIndex(20) + 0.1 * tile.getLatitudeStep(),
+ tile.getLongitudeAtIndex(14) + 0.2 * tile.getLongitudeStep(),
+ 120.0);
+ GeodeticPoint gpB = new GeodeticPoint(tile.getLatitudeAtIndex(20) + 0.7 * tile.getLatitudeStep(),
+ tile.getLongitudeAtIndex(14) + 0.9 * tile.getLongitudeStep(),
+ 130.0);
+
+ // the line from gpA to gpB should traverse the DEM twice within the tile
+ // we use the points in the two different orders to retrieve both solutions
+ GeodeticPoint gpIAB = tile.pixelIntersection(gpA, gpB, 20, 14);
+ checkInLine(gpA, gpB, gpIAB);
+ checkOnTile(tile, gpIAB);
+ GeodeticPoint gpIBA = tile.pixelIntersection(gpB, gpA, 20, 14);
+ checkInLine(gpA, gpB, gpIBA);
+ checkOnTile(tile, gpIBA);
+
+ // the two solutions are different
+ Assert.assertEquals(120.231, gpIAB.getAltitude(), 1.0e-3);
+ Assert.assertEquals(130.081, gpIBA.getAltitude(), 1.0e-3);
+
+ }
+
+ @Test
+ public void testPixelIntersectionNoSolutions() throws RuggedException {
+ SimpleTile tile = new SimpleTileFactory().createTile();
+ tile.setGeometry(0.0, 0.0, 0.025, 0.025, 50, 50);
+ tile.setElevation(20, 14, 91.0);
+ tile.setElevation(20, 15, 210.0);
+ tile.setElevation(21, 14, 162.0);
+ tile.setElevation(21, 15, 95.0);
+ tile.tileUpdateCompleted();
+ GeodeticPoint gpA = new GeodeticPoint(tile.getLatitudeAtIndex(20) + 0.1 * tile.getLatitudeStep(),
+ tile.getLongitudeAtIndex(14) + 0.2 * tile.getLongitudeStep(),
+ 180.0);
+ GeodeticPoint gpB = new GeodeticPoint(tile.getLatitudeAtIndex(20) + 0.7 * tile.getLatitudeStep(),
+ tile.getLongitudeAtIndex(14) + 0.9 * tile.getLongitudeStep(),
+ 190.0);
+
+ Assert.assertNull(tile.pixelIntersection(gpA, gpB, 20, 14));
+
+ }
+
private void checkOutOfBound(double latitude, double longitude, Tile tile) {
try {
tile.interpolateElevation(latitude, longitude);
@@ -188,4 +277,25 @@ public class SimpleTileTest {
}
}
+ private void checkInLine(GeodeticPoint gpA, GeodeticPoint gpB, GeodeticPoint gpI) {
+
+ double t = (gpI.getAltitude() - gpA.getAltitude()) / (gpB.getAltitude() - gpA.getAltitude());
+
+ Assert.assertEquals(gpI.getLatitude(),
+ gpA.getLatitude() * (1 - t) + gpB.getLatitude() * t,
+ 1.0e-10);
+
+ Assert.assertEquals(gpI.getLongitude(),
+ gpA.getLongitude() * (1 - t) + gpB.getLongitude() * t,
+ 1.0e-10);
+
+ }
+
+ private void checkOnTile(Tile tile, GeodeticPoint gpI)
+ throws RuggedException {
+ Assert.assertEquals(gpI.getAltitude(),
+ tile.interpolateElevation(gpI.getLatitude(), gpI.getLongitude()),
+ 1.0e-10);
+ }
+
}
--
GitLab