Added an option to use Duvenhage algorithm with flat-Earth hypothesis.

This option is mainly intended for comparison with existing systems. It
should not be used for operational systems and the full Duvenhage
algorithm with line-of-sight bending in geodetic coordinates should be
used instead.

src/main/java/org/orekit/rugged/api/AlgorithmId.java

@@ -31,6 +31,23 @@ public enum AlgorithmId {
      */
     DUVENHAGE,
 
+    /** Fast algorithm due to Bernardt Duvenhage.
+     * <p>
+     * The algorithm is described in the 2009 paper:
+     * <a href="http://researchspace.csir.co.za/dspace/bitstream/10204/3041/1/Duvenhage_2009.pdf">Using
+     * An Implicit Min/Max KD-Tree for Doing Efficient Terrain Line of Sight Calculations</a>.
+     * </p>
+     * <p>
+     * This version of the duvenhage's algorithm considers the body to be flat, i.e. lines computed
+     * from entry/exit points in the Digital Elevation Model are considered to be straight lines
+     * also in geodetic coordinates. The sagitta resulting from real ellipsoid curvature is therefore
+     * <em>not</em> corrected in this case. As this computation is not costly (a few percents overhead),
+     * the full {@link #DUVENHAGE} is recommended instead of this one. This choice is mainly intended
+     * for comparison purposes with other systems.
+     * </p>
+     */
+    DUVENHAGE_FLAT_BODY,
+
     /** Basic, <em>very slow</em> algorithm, designed only for tests and validation purposes.
      * <p>
      * The algorithm simply computes entry and exit points at high and low altitudes,

src/main/java/org/orekit/rugged/api/Rugged.java

@@ -128,9 +128,8 @@ public class Rugged {
      * @param updater updater used to load Digital Elevation Model tiles
      * @param maxCachedTiles maximum number of tiles stored in the cache
      * @param algorithmID identifier of algorithm to use for Digital Elevation Model intersection
-     * @param ellipsoidID identifier of reference ellipsoid
-     * @param inertialFrameID identifier of inertial frame
-     * @param bodyRotatingFrameID identifier of body rotating frame
+     * @param ellipsoid reference ellipsoid
+     * @param inertialFrame inertial frame
      * @param positionsVelocities satellite position and velocity
      * @param pvInterpolationOrder order to use for position/velocity interpolation
      * @param quaternions satellite quaternions
@@ -404,7 +403,7 @@ public class Rugged {
 
     }
 
-    /** Convert a {@link OneAxisEllipsoid} into a {@link ExtendedEllipsoid}
+    /** Convert a {@link OneAxisEllipsoid} into a {@link ExtendedEllipsoid}.
      * @param ellipsoid ellpsoid to extend
      * @return extended ellipsoid
      */
@@ -444,7 +443,8 @@ public class Rugged {
         // set up the ephemeris
         final List<Orbit> orbits = new ArrayList<Orbit>(positionsVelocities.size());
         for (final Pair<AbsoluteDate, PVCoordinates> pv : positionsVelocities) {
-            final CartesianOrbit orbit = new CartesianOrbit(pv.getSecond(), inertialFrame, pv.getFirst(), Constants.EIGEN5C_EARTH_MU);
+            final CartesianOrbit orbit = new CartesianOrbit(pv.getSecond(), inertialFrame,
+                                                            pv.getFirst(), Constants.EIGEN5C_EARTH_MU);
             orbits.add(orbit);
         }
 
@@ -455,7 +455,7 @@ public class Rugged {
             /** {@inhritDoc} */
             @Override
             public PVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame f)
-                    throws OrekitException {
+                throws OrekitException {
                 final List<Orbit> sample = cache.getNeighbors(date);
                 final Orbit interpolated = sample.get(0).interpolate(date, sample);
                 return interpolated.getPVCoordinates(date, f);
@@ -477,7 +477,9 @@ public class Rugged {
         // set up the algorithm
         switch (algorithmID) {
         case DUVENHAGE :
-            return new DuvenhageAlgorithm(updater, maxCachedTiles);
+            return new DuvenhageAlgorithm(updater, maxCachedTiles, false);
+        case DUVENHAGE_FLAT_BODY :
+            return new DuvenhageAlgorithm(updater, maxCachedTiles, true);
         case BASIC_SLOW_EXHAUSTIVE_SCAN_FOR_TESTS_ONLY :
             return new BasicScanAlgorithm(updater, maxCachedTiles);
         case IGNORE_DEM_USE_ELLIPSOID :

src/main/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithm.java

@@ -44,12 +44,23 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm {
     /** Cache for DEM tiles. */
     private final TilesCache<MinMaxTreeTile> cache;
 
+    /** Flag for flat-body hypothesis. */
+    private final boolean flatBody;
+
     /** Simple constructor.
      * @param updater updater used to load Digital Elevation Model tiles
      * @param maxCachedTiles maximum number of tiles stored in the cache
+     * @param flatBody if true, the body is considered flat, i.e. lines computed
+     * from entry/exit points in the DEM are considered to be straight lines also
+     * in geodetic coordinates. The sagitta resulting from real ellipsoid curvature
+     * is therefore <em>not</em> corrected in this case. As this computation is not
+     * costly (a few percents overhead), it is not recommended to set this parameter
+     * to {@code true}. This flag is mainly intended for comparison purposes with other systems.
      */
-    public DuvenhageAlgorithm(final TileUpdater updater, final int maxCachedTiles) {
-        cache = new TilesCache<MinMaxTreeTile>(new MinMaxTreeTileFactory(), updater, maxCachedTiles);
+    public DuvenhageAlgorithm(final TileUpdater updater, final int maxCachedTiles,
+                              final boolean flatBody) {
+        this.cache = new TilesCache<MinMaxTreeTile>(new MinMaxTreeTileFactory(), updater, maxCachedTiles);
+        this.flatBody = flatBody;
     }
 
     /** {@inheritDoc} */
@@ -174,7 +185,7 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm {
             // we have narrowed the search down to a single Digital Elevation Model pixel
             GeodeticPoint intersection = tile.pixelIntersection(entry, ellipsoid.convertLos(entry, los),
                                                                 exitLat, exitLon);
-            if (intersection != null) {
+            if (intersection != null && !flatBody) {
                 // improve the point, by projecting it back on the 3D line,
                 // fixing the small body curvature remaining at pixel level
                 final Vector3D      delta     = ellipsoid.transform(intersection).subtract(position);
@@ -212,20 +223,34 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm {
                 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());
+                    final GeodeticPoint crossingGP;
+                    if (flatBody) {
+                        // linear approximation of crossing point
+                        final double d  = exit.getLongitude() - entry.getLongitude();
+                        final double cN = (exit.getLongitude() - longitude) / d;
+                        final double cX = (longitude - entry.getLongitude()) / d;
+                        crossingGP = new GeodeticPoint(cN * entry.getLatitude() + cX * exit.getLatitude(),
+                                                       longitude,
+                                                       cN * entry.getAltitude() + cX * exit.getAltitude());
+                    } else {
+                        // full computation of crossing point
+                        final Vector3D crossingP = ellipsoid.pointAtLongitude(position, los, longitude);
+                        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(depth + 1, ellipsoid, position, los, tile,
-                                                                           previousGP, previousLat, previousLon,
-                                                                           crossingGP, crossingLat, crossingLonBefore);
-                    if (intersection != null) {
-                        return intersection;
+                    if (inRange(crossingLonBefore, entryLon, exitLon)) {
+                        // look for intersection
+                        final GeodeticPoint intersection = recurseIntersection(depth + 1, ellipsoid, position, los, tile,
+                                                                               previousGP, previousLat, previousLon,
+                                                                               crossingGP, crossingLat, crossingLonBefore);
+                        if (intersection != null) {
+                            return intersection;
+                        }
                     }
 
                     // prepare next segment
@@ -246,21 +271,35 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm {
                 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());
+                    final GeodeticPoint crossingGP;
+                    if (flatBody) {
+                        // linear approximation of crossing point
+                        final double d  = exit.getLatitude() - entry.getLatitude();
+                        final double cN = (exit.getLatitude() - latitude) / d;
+                        final double cX = (latitude - entry.getLatitude()) / d;
+                        crossingGP = new GeodeticPoint(latitude,
+                                                       cN * entry.getLongitude() + cX * exit.getLongitude(),
+                                                       cN * entry.getAltitude()  + cX * exit.getAltitude());
+                    } else {
+                        // full computation of crossing point
+                        final Vector3D crossingP = ellipsoid.pointAtLatitude(position, los,
+                                                                             tile.getLatitudeAtIndex(crossingLat));
+                        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(depth + 1, ellipsoid, position, los, tile,
-                                                                           previousGP, previousLat, previousLon,
-                                                                           crossingGP, crossingLatBefore, crossingLon);
-                    if (intersection != null) {
-                        return intersection;
+                    if (inRange(crossingLatBefore, entryLat, exitLat)) {
+                        // look for intersection
+                        final GeodeticPoint intersection = recurseIntersection(depth + 1, ellipsoid, position, los, tile,
+                                                                               previousGP, previousLat, previousLon,
+                                                                               crossingGP, crossingLatBefore, crossingLon);
+                        if (intersection != null) {
+                            return intersection;
+                        }
                     }
 
                     // prepare next segment
@@ -273,11 +312,25 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm {
             }
         }
 
-        // last part of the segment, up to exit point
-        return recurseIntersection(depth + 1, ellipsoid, position, los, tile,
-                                   previousGP, previousLat, previousLon,
-                                   exit, exitLat, exitLon);
+        if (inRange(previousLat, entryLat, exitLat) && inRange(previousLon, entryLon, exitLon)) {
+            // last part of the segment, up to exit point
+            return recurseIntersection(depth + 1, ellipsoid, position, los, tile,
+                                       previousGP, previousLat, previousLon,
+                                       exit, exitLat, exitLon);
+        } else {
+            return null;
+        }
+
+    }
 
+    /** Check if an index is inside a range.
+     * @param i index to check
+     * @param a first bound of the range (may be either below or above b)
+     * @param b second bound of the range (may be either below or above a)
+     * @return true if i is between a and b (inclusive)
+     */
+    private boolean inRange(final int i, final int a, final int b) {
+        return i >= FastMath.min(a, b) && i <= FastMath.max(a, b);
     }
 
     /** Compute a line-of-sight exit point from a tile.

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

@@ -31,6 +31,7 @@ import java.util.Locale;
 import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
 import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
 import org.apache.commons.math3.ode.nonstiff.DormandPrince853Integrator;
+import org.apache.commons.math3.stat.descriptive.SummaryStatistics;
 import org.apache.commons.math3.util.FastMath;
 import org.apache.commons.math3.util.Pair;
 import org.junit.Assert;
@@ -61,6 +62,7 @@ import org.orekit.propagation.Propagator;
 import org.orekit.propagation.SpacecraftState;
 import org.orekit.propagation.analytical.KeplerianPropagator;
 import org.orekit.propagation.numerical.NumericalPropagator;
+import org.orekit.rugged.core.raster.RandomLandscapeUpdater;
 import org.orekit.rugged.core.raster.VolcanicConeElevationUpdater;
 import org.orekit.time.AbsoluteDate;
 import org.orekit.time.TimeScalesFactory;
@@ -387,6 +389,73 @@ public class RuggedTest {
 
     }
 
+    @Test
+    public void testFlatBodyCorrection()
+        throws RuggedException, OrekitException, URISyntaxException {
+
+        int dimension = 200;
+
+        String path = getClass().getClassLoader().getResource("orekit-data").toURI().getPath();
+        DataProvidersManager.getInstance().addProvider(new DirectoryCrawler(new File(path)));
+        final BodyShape  earth = createEarth();
+        final Orbit      orbit = createOrbit(Constants.EIGEN5C_EARTH_MU);
+
+        AbsoluteDate crossing = new AbsoluteDate("2012-01-01T12:30:00.000", TimeScalesFactory.getUTC());
+
+        // one line sensor
+        // position: 1.5m in front (+X) and 20 cm above (-Z) of the S/C center of mass
+        // los: swath in the (YZ) plane, looking at 50° roll, ±1° aperture
+        List<PixelLOS> los = createLOS(new Vector3D(1.5, 0, -0.2),
+                                       new Rotation(Vector3D.PLUS_I, FastMath.toRadians(50.0)).applyTo(Vector3D.PLUS_K),
+                                       Vector3D.PLUS_I,
+                                       FastMath.toRadians(1.0), dimension);
+
+        // linear datation model: at reference time we get line 100, and the rate is one line every 1.5ms
+        LineDatation lineDatation = new LinearLineDatation(dimension / 2, 1.0 / 1.5e-3);
+        int firstLine = 0;
+        int lastLine  = dimension;
+
+        Propagator propagator = new KeplerianPropagator(orbit);
+        propagator.setAttitudeProvider(new YawCompensation(new NadirPointing(earth)));
+        propagator.propagate(crossing.shiftedBy(lineDatation.getDate(firstLine) - 1.0));
+        propagator.setEphemerisMode();
+        propagator.propagate(crossing.shiftedBy(lineDatation.getDate(lastLine) + 1.0));
+        Propagator ephemeris = propagator.getGeneratedEphemeris();
+
+        TileUpdater updater =
+                new RandomLandscapeUpdater(0.0, 9000.0, 0.5, 0xf0a401650191f9f6l,
+                                           FastMath.toRadians(1.0), 257);
+
+        Rugged ruggedFull = new Rugged(crossing, updater, 8,
+                                       AlgorithmId.DUVENHAGE,
+                                       EllipsoidId.WGS84,
+                                       InertialFrameId.EME2000,
+                                       BodyRotatingFrameId.ITRF,
+                                       ephemeris);
+        ruggedFull.setLineSensor("line", los, lineDatation);
+        GeodeticPoint[] gpWithFlatBodyCorrection = ruggedFull.directLocalization("line", 100);
+
+        Rugged ruggedFlat = new Rugged(crossing, updater, 8,
+                                       AlgorithmId.DUVENHAGE_FLAT_BODY,
+                                       EllipsoidId.WGS84,
+                                       InertialFrameId.EME2000,
+                                       BodyRotatingFrameId.ITRF,
+                                       ephemeris);
+        ruggedFlat.setLineSensor("line", los, lineDatation);
+        GeodeticPoint[] gpWithoutFlatBodyCorrection = ruggedFlat.directLocalization("line", 100);
+
+        SummaryStatistics stats = new SummaryStatistics();
+        for (int i = 0; i < gpWithFlatBodyCorrection.length; ++i) {
+            Vector3D pWith    = earth.transform(gpWithFlatBodyCorrection[i]);
+            Vector3D pWithout = earth.transform(gpWithoutFlatBodyCorrection[i]);
+            stats.addValue(Vector3D.distance(pWith, pWithout));
+        }
+        Assert.assertEquals(0.005, stats.getMin(),  1.0e-3);
+        Assert.assertEquals(6.503, stats.getMax(),  1.0e-3);
+        Assert.assertEquals(2.199, stats.getMean(), 1.0e-3);
+
+    }
+
     private BodyShape createEarth()
        throws OrekitException {
         return new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,

src/test/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithmTest.java

@@ -29,7 +29,7 @@ import org.orekit.rugged.core.raster.IntersectionAlgorithm;
 public class DuvenhageAlgorithmTest extends AbstractAlgorithmTest {
 
     protected IntersectionAlgorithm createAlgorithm(final TileUpdater updater, final int maxCachedTiles) {
-        return new DuvenhageAlgorithm(updater, maxCachedTiles);
+        return new DuvenhageAlgorithm(updater, maxCachedTiles, false);
     }
 
     @Test

src/test/java/org/orekit/rugged/core/raster/RandomLandscapeUpdater.java

+/* Copyright 2013-2014 CS Systèmes d'Information
+ * Licensed to CS Systèmes d'Information (CS) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * CS licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package org.orekit.rugged.core.raster;
+
+import java.io.IOException;
+
+import org.apache.commons.math3.exception.MathIllegalArgumentException;
+import org.apache.commons.math3.exception.util.LocalizedFormats;
+import org.apache.commons.math3.random.RandomGenerator;
+import org.apache.commons.math3.random.Well19937a;
+import org.apache.commons.math3.util.ArithmeticUtils;
+import org.apache.commons.math3.util.FastMath;
+import org.orekit.rugged.api.RuggedException;
+import org.orekit.rugged.api.TileUpdater;
+import org.orekit.rugged.api.UpdatableTile;
+
+public class RandomLandscapeUpdater implements TileUpdater {
+
+    private double size;
+    private int    n;
+    private double[][] heightMap;
+
+    public RandomLandscapeUpdater(double baseH, double initialScale, double reductionFactor,
+                                  long seed, double size, int n)
+        throws MathIllegalArgumentException {
+
+        if (!ArithmeticUtils.isPowerOfTwo(n - 1)) {
+            throw new MathIllegalArgumentException(LocalizedFormats.SIMPLE_MESSAGE,
+                                                   "tile size must be a power of two plus one");
+        }
+
+        this.size = size;
+        this.n    = n;
+
+        // as we want to use this for testing and comparison purposes,
+        // and as we don't control when tiles are generated, we generate
+        // only ONCE a height map with continuity at the edges, and
+        // reuse this single height map for ALL generated tiles
+        heightMap = new double[n][n];
+        RandomGenerator random  = new Well19937a(seed);
+
+        // initialize corners in diamond-square algorithm
+        heightMap[0][0]         = baseH;
+        heightMap[0][n - 1]     = baseH;
+        heightMap[n - 1][0]     = baseH;
+        heightMap[n - 1][n - 1] = baseH;
+
+        double scale = initialScale;
+        for (int span = n - 1; span > 1; span = span / 2) {
+
+            // perform squares step
+            for (int i = span / 2; i < n; i += span) {
+                for (int j = span / 2; j < n; j += span) {
+                    double middleH = mean(i - span / 2, j - span / 2,
+                                          i - span / 2, j + span / 2,
+                                          i + span / 2, j - span / 2,
+                                          i + span / 2, j + span / 2) +
+                                     scale * (random.nextDouble() - 0.5);
+                    heightMap[i][j] = middleH;
+                }
+            }
+
+            // perform diamonds step
+            boolean flipFlop = false;
+            for (int i = 0; i < n - 1; i += span / 2) {
+                for (int j = flipFlop ? 0 : span / 2; j < n - 1; j += span) {
+                    double middleH = mean(i - span / 2, j,
+                                          i + span / 2, j,
+                                          i,            j - span / 2,
+                                          i,            j + span / 2) +
+                                     scale * (random.nextDouble() - 0.5);
+                    heightMap[i][j] = middleH;
+                    if (i == 0) {
+                        heightMap[n - 1][j] = middleH;
+                    }
+                    if (j == 0) {
+                        heightMap[i][n - 1] = middleH;
+                    }
+                }
+                flipFlop = !flipFlop;
+            }
+
+            // reduce scale
+            scale *= reductionFactor;
+
+        }
+
+        try {
+            java.io.PrintStream out = new java.io.PrintStream("/home/luc/x.dat");
+            for (int i = 0; i < n; ++i) {
+                for (int j = 0; j < n; ++j) {
+                    out.format(java.util.Locale.US, "%d %d %12.3f%n",
+                               i, j, heightMap[i][j]);
+                }
+                out.format("%n");
+            }
+            out.close();
+        } catch (IOException ioe) {
+            ioe.printStackTrace(System.err);
+            System.exit(1);
+        }
+
+    }
+
+    public void updateTile(double latitude, double longitude, UpdatableTile tile)
+        throws RuggedException {
+
+        double step         = size / (n - 1);
+        double minLatitude  = size * FastMath.floor(latitude  / size);
+        double minLongitude = size * FastMath.floor(longitude / size);
+        tile.setGeometry(minLatitude, minLongitude, step, step, n, n);
+        for (int i = 0; i < n; ++i) {
+            for (int j = 0; j < n; ++j) {
+                tile.setElevation(i, j, heightMap[i][j]);
+            }
+        }
+
+    }
+
+    private double mean(int i1, int j1, int i2, int j2, int i3, int j3, int i4, int j4) {
+        return (heightMap[(i1 + n) % n][(j1 + n) % n] +
+                heightMap[(i2 + n) % n][(j2 + n) % n] +
+                heightMap[(i3 + n) % n][(j3 + n) % n] +
+                heightMap[(i4 + n) % n][(j4 + n) % n]) / 4;
+    }
+
+}