DuvenhageAlgorithm.java

  1. /* Copyright 2013-2017 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.rugged.intersection.duvenhage;

  19. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  20. import org.hipparchus.util.FastMath;
  21. import org.orekit.bodies.GeodeticPoint;
  22. import org.orekit.errors.OrekitException;
  23. import org.orekit.rugged.api.AlgorithmId;
  24. import org.orekit.rugged.errors.DumpManager;
  25. import org.orekit.rugged.errors.RuggedException;
  26. import org.orekit.rugged.errors.RuggedMessages;
  27. import org.orekit.rugged.intersection.IntersectionAlgorithm;
  28. import org.orekit.rugged.raster.Tile;
  29. import org.orekit.rugged.raster.TileUpdater;
  30. import org.orekit.rugged.raster.TilesCache;
  31. import org.orekit.rugged.utils.ExtendedEllipsoid;
  32. import org.orekit.rugged.utils.NormalizedGeodeticPoint;

  34. /** Digital Elevation Model intersection using Bernardt Duvenhage's algorithm.
  35.  * <p>
  36.  * The algorithm is described in the 2009 paper:
  37.  * <a href="http://researchspace.csir.co.za/dspace/bitstream/10204/3041/1/Duvenhage_2009.pdf">Using
  38.  * An Implicit Min/Max KD-Tree for Doing Efficient Terrain Line of Sight Calculations</a>.
  39.  * </p>
  40.  * @author Luc Maisonobe
  41.  */
  42. public class DuvenhageAlgorithm implements IntersectionAlgorithm {

  44.     /** Step size when skipping from one tile to a neighbor one, in meters. */
  45.     private static final double STEP = 0.01;

  47.     /** Cache for DEM tiles. */
  48.     private final TilesCache<MinMaxTreeTile> cache;

  50.     /** Flag for flat-body hypothesis. */
  51.     private final boolean flatBody;

  53.     /** Simple constructor.
  54.      * @param updater updater used to load Digital Elevation Model tiles
  55.      * @param maxCachedTiles maximum number of tiles stored in the cache
  56.      * @param flatBody if true, the body is considered flat, i.e. lines computed
  57.      * from entry/exit points in the DEM are considered to be straight lines also
  58.      * in geodetic coordinates. The sagitta resulting from real ellipsoid curvature
  59.      * is therefore <em>not</em> corrected in this case. As this computation is not
  60.      * costly (a few percents overhead), it is highly recommended to set this parameter
  61.      * to {@code false}. This flag is mainly intended for comparison purposes with other systems.
  62.      */
  63.     public DuvenhageAlgorithm(final TileUpdater updater, final int maxCachedTiles,
  64.                               final boolean flatBody) {
  65.         this.cache = new TilesCache<MinMaxTreeTile>(new MinMaxTreeTileFactory(), updater, maxCachedTiles);
  66.         this.flatBody = flatBody;
  67.     }

  69.     /** {@inheritDoc} */
  70.     @Override
  71.     public NormalizedGeodeticPoint intersection(final ExtendedEllipsoid ellipsoid,
  72.                                                 final Vector3D position, final Vector3D los)
  73.         throws RuggedException {
  74.         try {

  76.             DumpManager.dumpAlgorithm(flatBody ? AlgorithmId.DUVENHAGE_FLAT_BODY : AlgorithmId.DUVENHAGE);

  78.             // compute intersection with ellipsoid
  79.             final NormalizedGeodeticPoint gp0 = ellipsoid.pointOnGround(position, los, 0.0);

  81.             // compute atmosphere deviation

  83.             // locate the entry tile along the line-of-sight
  84.             MinMaxTreeTile tile = cache.getTile(gp0.getLatitude(), gp0.getLongitude());

  86.             NormalizedGeodeticPoint current = null;
  87.             double hMax = tile.getMaxElevation();
  88.             while (current == null) {

  90.                 // find where line-of-sight crosses tile max altitude
  91.                 final Vector3D entryP = ellipsoid.pointAtAltitude(position, los, hMax + STEP);
  92.                 if (Vector3D.dotProduct(entryP.subtract(position), los) < 0) {
  93.                     // the entry point is behind spacecraft!
  94.                     throw new RuggedException(RuggedMessages.DEM_ENTRY_POINT_IS_BEHIND_SPACECRAFT);
  95.                 }
  96.                 current = ellipsoid.transform(entryP, ellipsoid.getBodyFrame(), null, tile.getMinimumLongitude());

  98.                 if (tile.getLocation(current.getLatitude(), current.getLongitude()) != Tile.Location.HAS_INTERPOLATION_NEIGHBORS) {
  99.                     // the entry point is in another tile
  100.                     tile    = cache.getTile(current.getLatitude(), current.getLongitude());
  101.                     hMax    = FastMath.max(hMax, tile.getMaxElevation());
  102.                     current = null;
  103.                 }

  105.             }

  107.             // loop along the path
  108.             while (true) {

  110.                 // find where line-of-sight exit tile
  111.                 final LimitPoint exit = findExit(tile, ellipsoid, position, los);

  113.                 // compute intersection with Digital Elevation Model
  114.                 final int entryLat = FastMath.max(0,
  115.                                                   FastMath.min(tile.getLatitudeRows() - 1,
  116.                                                                tile.getFloorLatitudeIndex(current.getLatitude())));
  117.                 final int entryLon = FastMath.max(0,
  118.                                                   FastMath.min(tile.getLongitudeColumns() - 1,
  119.                                                                tile.getFloorLongitudeIndex(current.getLongitude())));
  120.                 final int exitLat  = FastMath.max(0,
  121.                                                   FastMath.min(tile.getLatitudeRows() - 1,
  122.                                                                tile.getFloorLatitudeIndex(exit.getPoint().getLatitude())));
  123.                 final int exitLon  = FastMath.max(0,
  124.                                                   FastMath.min(tile.getLongitudeColumns() - 1,
  125.                                                                tile.getFloorLongitudeIndex(exit.getPoint().getLongitude())));
  126.                 NormalizedGeodeticPoint intersection = recurseIntersection(0, ellipsoid, position, los, tile,
  127.                                                                            current, entryLat, entryLon,
  128.                                                                            exit.getPoint(), exitLat, exitLon);

  130.                 if (intersection != null) {
  131.                     // we have found the intersection
  132.                     return intersection;
  133.                 } else if (exit.atSide()) {
  134.                     // no intersection on this tile, we can proceed to next part of the line-of-sight

  136.                     // select next tile after current point
  137.                     final Vector3D forward = new Vector3D(1.0, ellipsoid.transform(exit.getPoint()), STEP, los);
  138.                     current = ellipsoid.transform(forward, ellipsoid.getBodyFrame(), null, tile.getMinimumLongitude());
  139.                     tile = cache.getTile(current.getLatitude(), current.getLongitude());

  141.                     if (tile.interpolateElevation(current.getLatitude(), current.getLongitude()) >= current.getAltitude()) {
  142.                         // extremely rare case! The line-of-sight traversed the Digital Elevation Model
  143.                         // during the very short forward step we used to move to next tile
  144.                         // we consider this point to be OK
  145.                         return current;
  146.                     }

  148.                 } else {

  150.                     // this should never happen
  151.                     // we should have left the loop with an intersection point
  152.                     // try a fallback non-recursive search
  153.                     intersection = noRecurseIntersection(ellipsoid, position, los, tile,
  154.                                                          current, entryLat, entryLon,
  155.                                                          exitLat, exitLon);
  156.                     if (intersection != null) {
  157.                         return intersection;
  158.                     } else {
  159.                         throw RuggedException.createInternalError(null);
  160.                     }

  162.                 }


  165.             }


  168.         } catch (OrekitException oe) {
  169.             throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
  170.         }
  171.     }

  173.     /** {@inheritDoc} */
  174.     @Override
  175.     public NormalizedGeodeticPoint refineIntersection(final ExtendedEllipsoid ellipsoid,
  176.                                                       final Vector3D position, final Vector3D los,
  177.                                                       final NormalizedGeodeticPoint closeGuess)
  178.         throws RuggedException {
  179.         try {

  181.             DumpManager.dumpAlgorithm(flatBody ? AlgorithmId.DUVENHAGE_FLAT_BODY : AlgorithmId.DUVENHAGE);

  183.             if (flatBody) {
  184.                 // under the (bad) flat-body assumption, the reference point must remain
  185.                 // at DEM entry and exit, even if we already have a much better close guess :-(
  186.                 // this is in order to remain consistent with other systems
  187.                 final Tile tile = cache.getTile(closeGuess.getLatitude(), closeGuess.getLongitude());
  188.                 final Vector3D      exitP  = ellipsoid.pointAtAltitude(position, los, tile.getMinElevation());
  189.                 final Vector3D      entryP = ellipsoid.pointAtAltitude(position, los, tile.getMaxElevation());
  190.                 final NormalizedGeodeticPoint entry  = ellipsoid.transform(entryP, ellipsoid.getBodyFrame(), null,
  191.                                                                            tile.getMinimumLongitude());
  192.                 return tile.cellIntersection(entry, ellipsoid.convertLos(entryP, exitP),
  193.                                               tile.getFloorLatitudeIndex(closeGuess.getLatitude()),
  194.                                               tile.getFloorLongitudeIndex(closeGuess.getLongitude()));
  195.             } else {
  196.                 final Vector3D      delta     = ellipsoid.transform(closeGuess).subtract(position);
  197.                 final double        s         = Vector3D.dotProduct(delta, los) / los.getNormSq();
  198.                 final GeodeticPoint projected = ellipsoid.transform(new Vector3D(1, position, s, los),
  199.                                                                     ellipsoid.getBodyFrame(), null);
  200.                 final NormalizedGeodeticPoint normalizedProjected = new NormalizedGeodeticPoint(projected.getLatitude(),
  201.                                                                                                 projected.getLongitude(),
  202.                                                                                                 projected.getAltitude(),
  203.                                                                                                 closeGuess.getLongitude());
  204.                 final Tile          tile      = cache.getTile(normalizedProjected.getLatitude(),
  205.                                                               normalizedProjected.getLongitude());
  206.                 return tile.cellIntersection(normalizedProjected, ellipsoid.convertLos(normalizedProjected, los),
  207.                                               tile.getFloorLatitudeIndex(normalizedProjected.getLatitude()),
  208.                                               tile.getFloorLongitudeIndex(normalizedProjected.getLongitude()));
  209.             }
  210.         } catch (OrekitException oe) {
  211.             throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
  212.         }
  213.     }

  215.     /** {@inheritDoc} */
  216.     @Override
  217.     public double getElevation(final double latitude, final double longitude)
  218.         throws RuggedException {
  219.         DumpManager.dumpAlgorithm(flatBody ? AlgorithmId.DUVENHAGE_FLAT_BODY : AlgorithmId.DUVENHAGE);
  220.         final Tile tile = cache.getTile(latitude, longitude);
  221.         return tile.interpolateElevation(latitude, longitude);
  222.     }

  224.     /** Compute intersection of line with Digital Elevation Model in a sub-tile.
  225.      * @param depth recursion depth
  226.      * @param ellipsoid reference ellipsoid
  227.      * @param position pixel position in ellipsoid frame
  228.      * @param los pixel line-of-sight in ellipsoid frame
  229.      * @param tile Digital Elevation Model tile
  230.      * @param entry line-of-sight entry point in the sub-tile
  231.      * @param entryLat index to use for interpolating entry point elevation
  232.      * @param entryLon index to use for interpolating entry point elevation
  233.      * @param exit line-of-sight exit point from the sub-tile
  234.      * @param exitLat index to use for interpolating exit point elevation
  235.      * @param exitLon index to use for interpolating exit point elevation
  236.      * @return point at which the line first enters ground, or null if does not enter
  237.      * ground in the search sub-tile
  238.      * @exception RuggedException if intersection cannot be found
  239.      * @exception OrekitException if points cannot be converted to geodetic coordinates
  240.      */
  241.     private NormalizedGeodeticPoint recurseIntersection(final int depth, final ExtendedEllipsoid ellipsoid,
  242.                                                         final Vector3D position, final Vector3D los,
  243.                                                         final MinMaxTreeTile tile,
  244.                                                         final NormalizedGeodeticPoint entry, final int entryLat, final int entryLon,
  245.                                                         final NormalizedGeodeticPoint exit, final int exitLat, final int exitLon)
  246.         throws RuggedException, OrekitException {

  248.         if (depth > 30) {
  249.             // this should never happen
  250.             throw RuggedException.createInternalError(null);
  251.         }

  253.         if (searchDomainSize(entryLat, entryLon, exitLat, exitLon) < 4) {
  254.             // we have narrowed the search down to a few cells
  255.             return noRecurseIntersection(ellipsoid, position, los, tile,
  256.                                          entry, entryLat, entryLon, exitLat, exitLon);
  257.         }

  259.         // find the deepest level in the min/max kd-tree at which entry and exit share a sub-tile
  260.         final int level = tile.getMergeLevel(entryLat, entryLon, exitLat, exitLon);
  261.         if (level >= 0  && exit.getAltitude() >= tile.getMaxElevation(exitLat, exitLon, level)) {
  262.             // the line-of-sight segment is fully above Digital Elevation Model
  263.             // we can safely reject it and proceed to next part of the line-of-sight
  264.             return null;
  265.         }

  267.         NormalizedGeodeticPoint previousGP    = entry;
  268.         int                     previousLat   = entryLat;
  269.         int                     previousLon   = entryLon;
  270.         final double            angularMargin = STEP / ellipsoid.getEquatorialRadius();

  272.         // introduce all intermediate points corresponding to the line-of-sight
  273.         // intersecting the boundary between level 0 sub-tiles
  274.         if (tile.isColumnMerging(level + 1)) {
  275.             // recurse through longitude crossings

  277.             final int[] crossings = tile.getCrossedBoundaryColumns(previousLon, exitLon, level + 1);
  278.             for (final int crossingLon : crossings) {

  280.                 // compute segment endpoints
  281.                 final double longitude = tile.getLongitudeAtIndex(crossingLon);
  282.                 if (longitude >= FastMath.min(entry.getLongitude(), exit.getLongitude()) - angularMargin &&
  283.                     longitude <= FastMath.max(entry.getLongitude(), exit.getLongitude()) + angularMargin) {

  285.                     NormalizedGeodeticPoint crossingGP = null;
  286.                     if (!flatBody) {
  287.                         try {
  288.                             // full computation of crossing point
  289.                             final Vector3D crossingP = ellipsoid.pointAtLongitude(position, los, longitude);
  290.                             crossingGP = ellipsoid.transform(crossingP, ellipsoid.getBodyFrame(), null,
  291.                                                              tile.getMinimumLongitude());
  292.                         } catch  (RuggedException re) {
  293.                             // in some very rare cases of numerical noise, we miss the crossing point
  294.                             crossingGP = null;
  295.                         }
  296.                     }
  297.                     if (crossingGP == null) {
  298.                         // linear approximation of crossing point
  299.                         final double d  = exit.getLongitude() - entry.getLongitude();
  300.                         final double cN = (exit.getLongitude() - longitude) / d;
  301.                         final double cX = (longitude - entry.getLongitude()) / d;
  302.                         crossingGP = new NormalizedGeodeticPoint(cN * entry.getLatitude() + cX * exit.getLatitude(),
  303.                                                                  longitude,
  304.                                                                  cN * entry.getAltitude() + cX * exit.getAltitude(),
  305.                                                                  tile.getMinimumLongitude());
  306.                     }
  307.                     final int crossingLat =
  308.                             FastMath.max(0,
  309.                                          FastMath.min(tile.getLatitudeRows() - 1,
  310.                                                       tile.getFloorLatitudeIndex(crossingGP.getLatitude())));

  312.                     // adjust indices as the crossing point is by definition between the sub-tiles
  313.                     final int crossingLonBefore = crossingLon - (entryLon <= exitLon ? 1 : 0);
  314.                     final int crossingLonAfter  = crossingLon - (entryLon <= exitLon ? 0 : 1);

  316.                     if (inRange(crossingLonBefore, entryLon, exitLon)) {
  317.                         // look for intersection
  318.                         final NormalizedGeodeticPoint intersection;
  319.                         if (searchDomainSize(previousLat, previousLon, crossingLat, crossingLonBefore) <
  320.                             searchDomainSize(entryLat, entryLon, exitLat, exitLon)) {
  321.                             intersection = recurseIntersection(depth + 1, ellipsoid, position, los, tile,
  322.                                                                previousGP, previousLat, previousLon,
  323.                                                                crossingGP, crossingLat, crossingLonBefore);
  324.                         } else {
  325.                             // we failed to reduce domain size, probably due to numerical problems
  326.                             intersection = noRecurseIntersection(ellipsoid, position, los, tile,
  327.                                                                  previousGP, previousLat, previousLon,
  328.                                                                  crossingLat, crossingLonBefore);
  329.                         }
  330.                         if (intersection != null) {
  331.                             return intersection;
  332.                         }
  333.                     }

  335.                     // prepare next segment
  336.                     previousGP  = crossingGP;
  337.                     previousLat = crossingLat;
  338.                     previousLon = crossingLonAfter;

  340.                 }

  342.             }
  343.         } else {
  344.             // recurse through latitude crossings
  345.             final int[] crossings = tile.getCrossedBoundaryRows(previousLat, exitLat, level + 1);
  346.             for (final int crossingLat : crossings) {

  348.                 // compute segment endpoints
  349.                 final double latitude = tile.getLatitudeAtIndex(crossingLat);
  350.                 if (latitude >= FastMath.min(entry.getLatitude(), exit.getLatitude()) - angularMargin &&
  351.                     latitude <= FastMath.max(entry.getLatitude(), exit.getLatitude()) + angularMargin) {

  353.                     NormalizedGeodeticPoint crossingGP = null;
  354.                     if (!flatBody) {
  355.                         // full computation of crossing point
  356.                         try {
  357.                             final Vector3D crossingP = ellipsoid.pointAtLatitude(position, los,
  358.                                                                                  tile.getLatitudeAtIndex(crossingLat),
  359.                                                                                  ellipsoid.transform(entry));
  360.                             crossingGP = ellipsoid.transform(crossingP, ellipsoid.getBodyFrame(), null,
  361.                                                              tile.getMinimumLongitude());
  362.                         } catch  (RuggedException re) {
  363.                             // in some very rare cases of numerical noise, we miss the crossing point
  364.                             crossingGP = null;
  365.                         }
  366.                     }
  367.                     if (crossingGP == null) {
  368.                         // linear approximation of crossing point
  369.                         final double d  = exit.getLatitude() - entry.getLatitude();
  370.                         final double cN = (exit.getLatitude() - latitude) / d;
  371.                         final double cX = (latitude - entry.getLatitude()) / d;
  372.                         crossingGP = new NormalizedGeodeticPoint(latitude,
  373.                                                                  cN * entry.getLongitude() + cX * exit.getLongitude(),
  374.                                                                  cN * entry.getAltitude()  + cX * exit.getAltitude(),
  375.                                                                  tile.getMinimumLongitude());
  376.                     }
  377.                     final int crossingLon =
  378.                             FastMath.max(0,
  379.                                          FastMath.min(tile.getLongitudeColumns() - 1,
  380.                                                       tile.getFloorLongitudeIndex(crossingGP.getLongitude())));

  382.                     // adjust indices as the crossing point is by definition between the sub-tiles
  383.                     final int crossingLatBefore = crossingLat - (entryLat <= exitLat ? 1 : 0);
  384.                     final int crossingLatAfter  = crossingLat - (entryLat <= exitLat ? 0 : 1);

  386.                     if (inRange(crossingLatBefore, entryLat, exitLat)) {
  387.                         // look for intersection
  388.                         final NormalizedGeodeticPoint intersection;
  389.                         if (searchDomainSize(previousLat, previousLon, crossingLatBefore, crossingLon) <
  390.                             searchDomainSize(entryLat, entryLon, exitLat, exitLon)) {
  391.                             intersection = recurseIntersection(depth + 1, ellipsoid, position, los, tile,
  392.                                                                previousGP, previousLat, previousLon,
  393.                                                                crossingGP, crossingLatBefore, crossingLon);
  394.                         } else {
  395.                             intersection = noRecurseIntersection(ellipsoid, position, los, tile,
  396.                                                                  previousGP, previousLat, previousLon,
  397.                                                                  crossingLatBefore, crossingLon);
  398.                         }
  399.                         if (intersection != null) {
  400.                             return intersection;
  401.                         }
  402.                     }

  404.                     // prepare next segment
  405.                     previousGP  = crossingGP;
  406.                     previousLat = crossingLatAfter;
  407.                     previousLon = crossingLon;

  409.                 }

  411.             }
  412.         }

  414.         if (inRange(previousLat, entryLat, exitLat) && inRange(previousLon, entryLon, exitLon)) {
  415.             // last part of the segment, up to exit point
  416.             if (searchDomainSize(previousLat, previousLon, exitLat, exitLon) <
  417.                 searchDomainSize(entryLat, entryLon, exitLat, exitLon)) {
  418.                 return recurseIntersection(depth + 1, ellipsoid, position, los, tile,
  419.                                            previousGP, previousLat, previousLon,
  420.                                            exit, exitLat, exitLon);
  421.             } else {
  422.                 return noRecurseIntersection(ellipsoid, position, los, tile,
  423.                                              previousGP, previousLat, previousLon,
  424.                                              exitLat, exitLon);
  425.             }
  426.         } else {
  427.             return null;
  428.         }

  430.     }

  432.     /** Compute intersection of line with Digital Elevation Model in a sub-tile, without recursion.
  433.      * @param ellipsoid reference ellipsoid
  434.      * @param position pixel position in ellipsoid frame
  435.      * @param los pixel line-of-sight in ellipsoid frame
  436.      * @param tile Digital Elevation Model tile
  437.      * @param entry line-of-sight entry point in the sub-tile
  438.      * @param entryLat index to use for interpolating entry point elevation
  439.      * @param entryLon index to use for interpolating entry point elevation
  440.      * @param exitLat index to use for interpolating exit point elevation
  441.      * @param exitLon index to use for interpolating exit point elevation
  442.      * @return point at which the line first enters ground, or null if does not enter
  443.      * ground in the search sub-tile
  444.      * @exception RuggedException if intersection cannot be found
  445.      * @exception OrekitException if points cannot be converted to geodetic coordinates
  446.      */
  447.     private NormalizedGeodeticPoint noRecurseIntersection(final ExtendedEllipsoid ellipsoid,
  448.                                                           final Vector3D position, final Vector3D los,
  449.                                                           final MinMaxTreeTile tile,
  450.                                                           final NormalizedGeodeticPoint entry,
  451.                                                           final int entryLat, final int entryLon,
  452.                                                           final int exitLat, final int exitLon)
  453.         throws OrekitException, RuggedException {

  455.         NormalizedGeodeticPoint intersectionGP = null;
  456.         double intersectionDot = Double.POSITIVE_INFINITY;
  457.         for (int i = FastMath.min(entryLat, exitLat); i <= FastMath.max(entryLat, exitLat); ++i) {
  458.             for (int j = FastMath.min(entryLon, exitLon); j <= FastMath.max(entryLon, exitLon); ++j) {
  459.                 final NormalizedGeodeticPoint gp = tile.cellIntersection(entry, ellipsoid.convertLos(entry, los), i, j);
  460.                 if (gp != null) {

  462.                     // improve the point, by projecting it back on the 3D line, fixing the small body curvature at cell level
  463.                     final Vector3D      delta     = ellipsoid.transform(gp).subtract(position);
  464.                     final double        s         = Vector3D.dotProduct(delta, los) / los.getNormSq();
  465.                     final GeodeticPoint projected = ellipsoid.transform(new Vector3D(1, position, s, los),
  466.                                                                         ellipsoid.getBodyFrame(), null);
  467.                     final NormalizedGeodeticPoint normalizedProjected = new NormalizedGeodeticPoint(projected.getLatitude(),
  468.                                                                                                     projected.getLongitude(),
  469.                                                                                                     projected.getAltitude(),
  470.                                                                                                     gp.getLongitude());
  471.                     final NormalizedGeodeticPoint gpImproved = tile.cellIntersection(normalizedProjected,
  472.                                                                                      ellipsoid.convertLos(normalizedProjected, los),
  473.                                                                                      i, j);

  475.                     if (gpImproved != null) {
  476.                         final Vector3D point = ellipsoid.transform(gpImproved);
  477.                         final double dot = Vector3D.dotProduct(point.subtract(position), los);
  478.                         if (dot < intersectionDot) {
  479.                             intersectionGP  = gpImproved;
  480.                             intersectionDot = dot;
  481.                         }
  482.                     }

  484.                 }
  485.             }
  486.         }

  488.         return intersectionGP;

  490.     }

  492.     /** Compute the size of a search domain.
  493.      * @param entryLat index to use for interpolating entry point elevation
  494.      * @param entryLon index to use for interpolating entry point elevation
  495.      * @param exitLat index to use for interpolating exit point elevation
  496.      * @param exitLon index to use for interpolating exit point elevation
  497.      * @return size of the search domain
  498.      */
  499.     private int searchDomainSize(final int entryLat, final int entryLon,
  500.                                  final int exitLat, final int exitLon) {
  501.         return (FastMath.abs(entryLat - exitLat) + 1) * (FastMath.abs(entryLon - exitLon) + 1);
  502.     }

  504.     /** Check if an index is inside a range.
  505.      * @param i index to check
  506.      * @param a first bound of the range (may be either below or above b)
  507.      * @param b second bound of the range (may be either below or above a)
  508.      * @return true if i is between a and b (inclusive)
  509.      */
  510.     private boolean inRange(final int i, final int a, final int b) {
  511.         return i >= FastMath.min(a, b) && i <= FastMath.max(a, b);
  512.     }

  514.     /** Compute a line-of-sight exit point from a tile.
  515.      * @param tile tile to consider
  516.      * @param ellipsoid reference ellipsoid
  517.      * @param position pixel position in ellipsoid frame
  518.      * @param los pixel line-of-sight in ellipsoid frame
  519.      * @return exit point
  520.      * @exception RuggedException if exit point cannot be found
  521.      * @exception OrekitException if geodetic coordinates cannot be computed
  522.      */
  523.     private LimitPoint findExit(final Tile tile, final ExtendedEllipsoid ellipsoid,
  524.                                 final Vector3D position, final Vector3D los)
  525.         throws RuggedException, OrekitException {

  527.         // look for an exit at bottom
  528.         final double                  reference = tile.getMinimumLongitude();
  529.         final Vector3D                exitP     = ellipsoid.pointAtAltitude(position, los, tile.getMinElevation() - STEP);
  530.         final NormalizedGeodeticPoint exitGP    = ellipsoid.transform(exitP, ellipsoid.getBodyFrame(), null, reference);

  532.         switch (tile.getLocation(exitGP.getLatitude(), exitGP.getLongitude())) {
  533.             case SOUTH_WEST :
  534.                 return new LimitPoint(ellipsoid, reference,
  535.                                       selectClosest(latitudeCrossing(ellipsoid, position,  los, tile.getMinimumLatitude(),  exitP),
  536.                                                     longitudeCrossing(ellipsoid, position, los, tile.getMinimumLongitude(), exitP),
  537.                                                     position),
  538.                                       true);
  539.             case WEST :
  540.                 return new LimitPoint(ellipsoid, reference,
  541.                                       longitudeCrossing(ellipsoid, position, los, tile.getMinimumLongitude(), exitP),
  542.                                       true);
  543.             case NORTH_WEST:
  544.                 return new LimitPoint(ellipsoid, reference,
  545.                                       selectClosest(latitudeCrossing(ellipsoid, position,  los, tile.getMaximumLatitude(),  exitP),
  546.                                                     longitudeCrossing(ellipsoid, position, los, tile.getMinimumLongitude(), exitP),
  547.                                                     position),
  548.                                       true);
  549.             case NORTH :
  550.                 return new LimitPoint(ellipsoid, reference,
  551.                                       latitudeCrossing(ellipsoid, position, los, tile.getMaximumLatitude(), exitP),
  552.                                       true);
  553.             case NORTH_EAST :
  554.                 return new LimitPoint(ellipsoid, reference,
  555.                                       selectClosest(latitudeCrossing(ellipsoid, position,  los, tile.getMaximumLatitude(),  exitP),
  556.                                                     longitudeCrossing(ellipsoid, position, los, tile.getMaximumLongitude(), exitP),
  557.                                                     position),
  558.                                       true);
  559.             case EAST :
  560.                 return new LimitPoint(ellipsoid, reference,
  561.                                       longitudeCrossing(ellipsoid, position, los, tile.getMaximumLongitude(), exitP),
  562.                                       true);
  563.             case SOUTH_EAST :
  564.                 return new LimitPoint(ellipsoid, reference,
  565.                                       selectClosest(latitudeCrossing(ellipsoid, position,  los, tile.getMinimumLatitude(),  exitP),
  566.                                                     longitudeCrossing(ellipsoid, position, los, tile.getMaximumLongitude(), exitP),
  567.                                                     position),
  568.                                       true);
  569.             case SOUTH :
  570.                 return new LimitPoint(ellipsoid, reference,
  571.                                       latitudeCrossing(ellipsoid, position, los, tile.getMinimumLatitude(), exitP),
  572.                                       true);
  573.             case HAS_INTERPOLATION_NEIGHBORS :
  574.                 return new LimitPoint(exitGP, false);

  576.             default :
  577.                 // this should never happen
  578.                 throw RuggedException.createInternalError(null);
  579.         }

  581.     }

  583.     /** Select point closest to line-of-sight start.
  584.      * @param p1 first point to consider
  585.      * @param p2 second point to consider
  586.      * @param position pixel position in ellipsoid frame
  587.      * @return either p1 or p2, depending on which is closest to position
  588.      */
  589.     private Vector3D selectClosest(final Vector3D p1, final Vector3D p2, final Vector3D position) {
  590.         return Vector3D.distance(p1, position) <= Vector3D.distance(p2, position) ? p1 : p2;
  591.     }

  593.     /** Get point at some latitude along a pixel line of sight.
  594.      * @param ellipsoid reference ellipsoid
  595.      * @param position pixel position (in body frame)
  596.      * @param los pixel line-of-sight, not necessarily normalized (in body frame)
  597.      * @param latitude latitude with respect to ellipsoid
  598.      * @param closeReference reference point used to select the closest solution
  599.      * when there are two points at the desired latitude along the line
  600.      * @return point at altitude, or closeReference if no such point can be found
  601.      */
  602.     private Vector3D latitudeCrossing(final ExtendedEllipsoid ellipsoid,
  603.                                       final Vector3D position, final Vector3D los,
  604.                                       final double latitude, final Vector3D closeReference) {
  605.         try {
  606.             return ellipsoid.pointAtLatitude(position, los, latitude, closeReference);
  607.         } catch (RuggedException re) {
  608.             return closeReference;
  609.         }
  610.     }

  612.     /** Get point at some latitude along a pixel line of sight.
  613.      * @param ellipsoid reference ellipsoid
  614.      * @param position pixel position (in body frame)
  615.      * @param los pixel line-of-sight, not necessarily normalized (in body frame)
  616.      * @param longitude longitude with respect to ellipsoid
  617.      * @param closeReference reference point used to select the closest solution
  618.      * when there are two points at the desired latitude along the line
  619.      * @return point at altitude, or closeReference if no such point can be found
  620.      */
  621.     private Vector3D longitudeCrossing(final ExtendedEllipsoid ellipsoid,
  622.                                        final Vector3D position, final Vector3D los,
  623.                                        final double longitude, final Vector3D closeReference) {
  624.         try {
  625.             return ellipsoid.pointAtLongitude(position, los, longitude);
  626.         } catch (RuggedException re) {
  627.             return closeReference;
  628.         }
  629.     }

  631.     /** Point at tile boundary. */
  632.     private static class LimitPoint {

  634.         /** Coordinates. */
  635.         private final NormalizedGeodeticPoint point;

  637.         /** Limit status. */
  638.         private final boolean side;

  640.         /** Simple constructor.
  641.          * @param ellipsoid reference ellipsoid
  642.          * @param referenceLongitude reference longitude lc such that the point longitude will
  643.          * be normalized between lc-π and lc+π
  644.          * @param cartesian Cartesian point
  645.          * @param side if true, the point is on a side limit, otherwise
  646.          * it is on a top/bottom limit
  647.          * @exception OrekitException if geodetic coordinates cannot be computed
  648.          */
  649.         LimitPoint(final ExtendedEllipsoid ellipsoid, final double referenceLongitude,
  650.                    final Vector3D cartesian, final boolean side)
  651.             throws OrekitException {
  652.             this(ellipsoid.transform(cartesian, ellipsoid.getBodyFrame(), null, referenceLongitude), side);
  653.         }

  655.         /** Simple constructor.
  656.          * @param point coordinates
  657.          * @param side if true, the point is on a side limit, otherwise
  658.          * it is on a top/bottom limit
  659.          */
  660.         LimitPoint(final NormalizedGeodeticPoint point, final boolean side) {
  661.             this.point = point;
  662.             this.side  = side;
  663.         }

  665.         /** Get the point coordinates.
  666.          * @return point coordinates
  667.          */
  668.         public NormalizedGeodeticPoint getPoint() {
  669.             return point;
  670.         }

  672.         /** Check if point is on the side of a tile.
  673.          * @return true if the point is on a side limit, otherwise
  674.          * it is on a top/bottom limit
  675.          */
  676.         public boolean atSide() {
  677.             return side;
  678.         }

  680.     }

  682. }
Created with JaCoCo 0.7.9.201702052155