DuvenhageAlgorithm.java

  1. /* Copyright 2013-2019 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.rugged.api.AlgorithmId;
  23. import org.orekit.rugged.errors.DumpManager;
  24. import org.orekit.rugged.errors.RuggedException;
  25. import org.orekit.rugged.errors.RuggedInternalError;
  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.  * @author Guylaine Prat
  42.  */
  43. public class DuvenhageAlgorithm implements IntersectionAlgorithm {

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

  48.     /** Maximum number of attempts to refine intersection.
  49.      * <p>
  50.      * This parameter is intended to prevent infinite loops.
  51.      * </p>
  52.      * @since 2.1 */
  53.     private static final int MAX_REFINING_ATTEMPTS = 100;

  55.     /** Cache for DEM tiles. */
  56.     private final TilesCache<MinMaxTreeTile> cache;

  58.     /** Flag for flat-body hypothesis. */
  59.     private final boolean flatBody;

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

  77.     /** {@inheritDoc} */
  78.     @Override
  79.     public NormalizedGeodeticPoint intersection(final ExtendedEllipsoid ellipsoid,
  80.                                                 final Vector3D position, final Vector3D los) {

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

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

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

  90.         NormalizedGeodeticPoint current = null;
  91.         double hMax = tile.getMaxElevation();
  92.         while (current == null) {

  94.             // find where line-of-sight crosses tile max altitude
  95.             final Vector3D entryP = ellipsoid.pointAtAltitude(position, los, hMax + STEP);
  96.             if (Vector3D.dotProduct(entryP.subtract(position), los) < 0) {
  97.                 // the entry point is behind spacecraft!

  99.                 // let's see if at least we are above DEM
  100.                 try {
  101.                     final NormalizedGeodeticPoint positionGP =
  102.                                     ellipsoid.transform(position, ellipsoid.getBodyFrame(), null, tile.getMinimumLongitude());
  103.                     final double elevationAtPosition = tile.interpolateElevation(positionGP.getLatitude(), positionGP.getLongitude());
  104.                     if (positionGP.getAltitude() >= elevationAtPosition) {
  105.                         // we can use the current position as the entry point
  106.                         current = positionGP;
  107.                     } else {
  108.                         current = null;
  109.                     }
  110.                 } catch (RuggedException re) {
  111.                     if (re.getSpecifier() == RuggedMessages.OUT_OF_TILE_ANGLES) {
  112.                         current = null;
  113.                     }
  114.                 }

  116.                 if (current == null) {
  117.                     throw new RuggedException(RuggedMessages.DEM_ENTRY_POINT_IS_BEHIND_SPACECRAFT);
  118.                 }

  120.             } else {
  121.                 current = ellipsoid.transform(entryP, ellipsoid.getBodyFrame(), null, tile.getMinimumLongitude());
  122.             }

  124.             if (tile.getLocation(current.getLatitude(), current.getLongitude()) != Tile.Location.HAS_INTERPOLATION_NEIGHBORS) {
  125.                 // the entry point is in another tile
  126.                 tile    = cache.getTile(current.getLatitude(), current.getLongitude());
  127.                 hMax    = FastMath.max(hMax, tile.getMaxElevation());
  128.                 current = null;
  129.             }

  131.         }

  133.         // loop along the path
  134.         while (true) {

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

  139.             // compute intersection with Digital Elevation Model
  140.             final int entryLat = FastMath.max(0,
  141.                                               FastMath.min(tile.getLatitudeRows() - 1,
  142.                                                            tile.getFloorLatitudeIndex(current.getLatitude())));
  143.             final int entryLon = FastMath.max(0,
  144.                                               FastMath.min(tile.getLongitudeColumns() - 1,
  145.                                                            tile.getFloorLongitudeIndex(current.getLongitude())));
  146.             final int exitLat  = FastMath.max(0,
  147.                                               FastMath.min(tile.getLatitudeRows() - 1,
  148.                                                            tile.getFloorLatitudeIndex(exit.getPoint().getLatitude())));
  149.             final int exitLon  = FastMath.max(0,
  150.                                               FastMath.min(tile.getLongitudeColumns() - 1,
  151.                                                            tile.getFloorLongitudeIndex(exit.getPoint().getLongitude())));
  152.             NormalizedGeodeticPoint intersection = recurseIntersection(0, ellipsoid, position, los, tile,
  153.                                                                        current, entryLat, entryLon,
  154.                                                                        exit.getPoint(), exitLat, exitLon);

  156.             if (intersection != null) {
  157.                 // we have found the intersection
  158.                 return intersection;
  159.             } else if (exit.atSide()) {
  160.                 // no intersection on this tile, we can proceed to next part of the line-of-sight

  162.                 // select next tile after current point
  163.                 final Vector3D forward = new Vector3D(1.0, ellipsoid.transform(exit.getPoint()), STEP, los);
  164.                 current = ellipsoid.transform(forward, ellipsoid.getBodyFrame(), null, tile.getMinimumLongitude());
  165.                 tile = cache.getTile(current.getLatitude(), current.getLongitude());

  167.                 if (tile.interpolateElevation(current.getLatitude(), current.getLongitude()) >= current.getAltitude()) {
  168.                     // extremely rare case! The line-of-sight traversed the Digital Elevation Model
  169.                     // during the very short forward step we used to move to next tile
  170.                     // we consider this point to be OK
  171.                     return current;
  172.                 }

  174.             } else {

  176.                 // this should never happen
  177.                 // we should have left the loop with an intersection point
  178.                 // try a fallback non-recursive search
  179.                 intersection = noRecurseIntersection(ellipsoid, position, los, tile,
  180.                                                      current, entryLat, entryLon,
  181.                                                      exitLat, exitLon);
  182.                 if (intersection != null) {
  183.                     return intersection;
  184.                 } else {
  185.                     throw new RuggedInternalError(null);
  186.                 }
  187.             }
  188.         }
  189.     }

  191.     /** {@inheritDoc} */
  192.     @Override
  193.     public NormalizedGeodeticPoint refineIntersection(final ExtendedEllipsoid ellipsoid,
  194.                                                       final Vector3D position, final Vector3D los,
  195.                                                       final NormalizedGeodeticPoint closeGuess) {

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

  199.         if (flatBody) {
  200.             // under the (bad) flat-body assumption, the reference point must remain
  201.             // at DEM entry and exit, even if we already have a much better close guess :-(
  202.             // this is in order to remain consistent with other systems
  203.             final Tile tile = cache.getTile(closeGuess.getLatitude(), closeGuess.getLongitude());
  204.             final Vector3D      exitP  = ellipsoid.pointAtAltitude(position, los, tile.getMinElevation());
  205.             final Vector3D      entryP = ellipsoid.pointAtAltitude(position, los, tile.getMaxElevation());
  206.             final NormalizedGeodeticPoint entry  = ellipsoid.transform(entryP, ellipsoid.getBodyFrame(), null,
  207.                                                                        tile.getMinimumLongitude());
  208.             return tile.cellIntersection(entry, ellipsoid.convertLos(entryP, exitP),
  209.                                          tile.getFloorLatitudeIndex(closeGuess.getLatitude()),
  210.                                          tile.getFloorLongitudeIndex(closeGuess.getLongitude()));

  212.         } else {
  213.             // regular curved ellipsoid model

  215.             NormalizedGeodeticPoint currentGuess = closeGuess;

  217.             // normally, we should succeed at first attempt but in very rare cases
  218.             // we may loose the intersection (typically because some corrections introduced
  219.             // between the first intersection and the refining have slightly changed the
  220.             // relative geometry between Digital Elevation Model and Line Of Sight).
  221.             // In these rare cases, we have to recover a new intersection
  222.             for (int i = 0; i < MAX_REFINING_ATTEMPTS; ++i) {

  224.                 final Vector3D      delta      = ellipsoid.transform(currentGuess).subtract(position);
  225.                 final double        s          = Vector3D.dotProduct(delta, los) / los.getNormSq();
  226.                 final Vector3D      projectedP = new Vector3D(1, position, s, los);
  227.                 final GeodeticPoint projected  = ellipsoid.transform(projectedP, ellipsoid.getBodyFrame(), null);
  228.                 final NormalizedGeodeticPoint normalizedProjected =
  229.                         new NormalizedGeodeticPoint(projected.getLatitude(),
  230.                                                     projected.getLongitude(),
  231.                                                     projected.getAltitude(),
  232.                                                     currentGuess.getLongitude());
  233.                 final Tile tile = cache.getTile(normalizedProjected.getLatitude(), normalizedProjected.getLongitude());

  235.                 final Vector3D                topoLOS           = ellipsoid.convertLos(normalizedProjected, los);
  236.                 final int                     iLat              = tile.getFloorLatitudeIndex(normalizedProjected.getLatitude());
  237.                 final int                     iLon              = tile.getFloorLongitudeIndex(normalizedProjected.getLongitude());
  238.                 final NormalizedGeodeticPoint foundIntersection = tile.cellIntersection(normalizedProjected, topoLOS, iLat, iLon);

  240.                 if (foundIntersection != null) {
  241.                     // nominal case, we were able to refine the intersection
  242.                     return foundIntersection;
  243.                 } else {
  244.                     // extremely rare case: we have lost the intersection

  246.                     // find a start point for new search, leaving the current cell behind
  247.                     final double cellBoundaryLatitude  = tile.getLatitudeAtIndex(topoLOS.getY()  <= 0 ? iLat : iLat + 1);
  248.                     final double cellBoundaryLongitude = tile.getLongitudeAtIndex(topoLOS.getX() <= 0 ? iLon : iLon + 1);
  249.                     final Vector3D cellExit = new Vector3D(1, selectClosest(latitudeCrossing(ellipsoid, projectedP,  los, cellBoundaryLatitude,  projectedP),
  250.                                                                             longitudeCrossing(ellipsoid, projectedP, los, cellBoundaryLongitude, projectedP),
  251.                                                                             projectedP),
  252.                                                            STEP, los);
  253.                     final GeodeticPoint egp = ellipsoid.transform(cellExit, ellipsoid.getBodyFrame(), null);
  254.                     final NormalizedGeodeticPoint cellExitGP = new NormalizedGeodeticPoint(egp.getLatitude(),
  255.                                                                                            egp.getLongitude(),
  256.                                                                                            egp.getAltitude(),
  257.                                                                                            currentGuess.getLongitude());
  258.                     if (tile.interpolateElevation(cellExitGP.getLatitude(), cellExitGP.getLongitude()) >= cellExitGP.getAltitude()) {
  259.                         // extremely rare case! The line-of-sight traversed the Digital Elevation Model
  260.                         // during the very short forward step we used to move to next cell
  261.                         // we consider this point to be OK
  262.                         return cellExitGP;
  263.                     }


  266.                     // We recompute fully a new guess, starting from the point after current cell
  267.                     final GeodeticPoint currentGuessGP = intersection(ellipsoid, cellExit, los);
  268.                     currentGuess = new NormalizedGeodeticPoint(currentGuessGP.getLatitude(),
  269.                                                                currentGuessGP.getLongitude(),
  270.                                                                currentGuessGP.getAltitude(),
  271.                                                                projected.getLongitude());
  272.                 }

  274.             }

  276.             // no intersection found
  277.             return null;

  279.         } // end test on flatbody

  281.     }

  283.     /** {@inheritDoc} */
  284.     @Override
  285.     public double getElevation(final double latitude, final double longitude) {

  287.         DumpManager.dumpAlgorithm(flatBody ? AlgorithmId.DUVENHAGE_FLAT_BODY : AlgorithmId.DUVENHAGE);
  288.         final Tile tile = cache.getTile(latitude, longitude);
  289.         return tile.interpolateElevation(latitude, longitude);
  290.     }

  292.     /** Compute intersection of line with Digital Elevation Model in a sub-tile.
  293.      * @param depth recursion depth
  294.      * @param ellipsoid reference ellipsoid
  295.      * @param position pixel position in ellipsoid frame
  296.      * @param los pixel line-of-sight in ellipsoid frame
  297.      * @param tile Digital Elevation Model tile
  298.      * @param entry line-of-sight entry point in the sub-tile
  299.      * @param entryLat index to use for interpolating entry point elevation
  300.      * @param entryLon index to use for interpolating entry point elevation
  301.      * @param exit line-of-sight exit point from the sub-tile
  302.      * @param exitLat index to use for interpolating exit point elevation
  303.      * @param exitLon index to use for interpolating exit point elevation
  304.      * @return point at which the line first enters ground, or null if does not enter
  305.      * ground in the search sub-tile
  306.      */
  307.     private NormalizedGeodeticPoint recurseIntersection(final int depth, final ExtendedEllipsoid ellipsoid,
  308.                                                         final Vector3D position, final Vector3D los,
  309.                                                         final MinMaxTreeTile tile,
  310.                                                         final NormalizedGeodeticPoint entry, final int entryLat, final int entryLon,
  311.                                                         final NormalizedGeodeticPoint exit, final int exitLat, final int exitLon) {

  313.         if (depth > 30) {
  314.             // this should never happen
  315.             throw new RuggedInternalError(null);
  316.         }

  318.         if (searchDomainSize(entryLat, entryLon, exitLat, exitLon) < 4) {
  319.             // we have narrowed the search down to a few cells
  320.             return noRecurseIntersection(ellipsoid, position, los, tile,
  321.                                          entry, entryLat, entryLon, exitLat, exitLon);
  322.         }

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

  332.         NormalizedGeodeticPoint previousGP    = entry;
  333.         int                     previousLat   = entryLat;
  334.         int                     previousLon   = entryLon;
  335.         final double            angularMargin = STEP / ellipsoid.getEquatorialRadius();

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

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

  345.                 // compute segment endpoints
  346.                 final double longitude = tile.getLongitudeAtIndex(crossingLon);
  347.                 if (longitude >= FastMath.min(entry.getLongitude(), exit.getLongitude()) - angularMargin &&
  348.                     longitude <= FastMath.max(entry.getLongitude(), exit.getLongitude()) + angularMargin) {

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

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

  381.                     if (inRange(crossingLonBefore, entryLon, exitLon)) {
  382.                         // look for intersection
  383.                         final NormalizedGeodeticPoint intersection;
  384.                         if (searchDomainSize(previousLat, previousLon, crossingLat, crossingLonBefore) <
  385.                             searchDomainSize(entryLat, entryLon, exitLat, exitLon)) {
  386.                             intersection = recurseIntersection(depth + 1, ellipsoid, position, los, tile,
  387.                                                                previousGP, previousLat, previousLon,
  388.                                                                crossingGP, crossingLat, crossingLonBefore);
  389.                         } else {
  390.                             // we failed to reduce domain size, probably due to numerical problems
  391.                             intersection = noRecurseIntersection(ellipsoid, position, los, tile,
  392.                                                                  previousGP, previousLat, previousLon,
  393.                                                                  crossingLat, crossingLonBefore);
  394.                         }
  395.                         if (intersection != null) {
  396.                             return intersection;
  397.                         }
  398.                     }

  400.                     // prepare next segment
  401.                     previousGP  = crossingGP;
  402.                     previousLat = crossingLat;
  403.                     previousLon = crossingLonAfter;

  405.                 }

  407.             }
  408.         } else {
  409.             // recurse through latitude crossings
  410.             final int[] crossings = tile.getCrossedBoundaryRows(previousLat, exitLat, level + 1);
  411.             for (final int crossingLat : crossings) {

  413.                 // compute segment endpoints
  414.                 final double latitude = tile.getLatitudeAtIndex(crossingLat);
  415.                 if (latitude >= FastMath.min(entry.getLatitude(), exit.getLatitude()) - angularMargin &&
  416.                     latitude <= FastMath.max(entry.getLatitude(), exit.getLatitude()) + angularMargin) {

  418.                     NormalizedGeodeticPoint crossingGP = null;
  419.                     if (!flatBody) {
  420.                         // full computation of crossing point
  421.                         try {
  422.                             final Vector3D crossingP = ellipsoid.pointAtLatitude(position, los,
  423.                                                                                  tile.getLatitudeAtIndex(crossingLat),
  424.                                                                                  ellipsoid.transform(entry));
  425.                             crossingGP = ellipsoid.transform(crossingP, ellipsoid.getBodyFrame(), null,
  426.                                                              tile.getMinimumLongitude());
  427.                         } catch (RuggedException re) {
  428.                             // in some very rare cases of numerical noise, we miss the crossing point
  429.                             crossingGP = null;
  430.                         }
  431.                     }
  432.                     if (crossingGP == null) {
  433.                         // linear approximation of crossing point
  434.                         final double d  = exit.getLatitude() - entry.getLatitude();
  435.                         final double cN = (exit.getLatitude() - latitude) / d;
  436.                         final double cX = (latitude - entry.getLatitude()) / d;
  437.                         crossingGP = new NormalizedGeodeticPoint(latitude,
  438.                                                                  cN * entry.getLongitude() + cX * exit.getLongitude(),
  439.                                                                  cN * entry.getAltitude()  + cX * exit.getAltitude(),
  440.                                                                  tile.getMinimumLongitude());
  441.                     }
  442.                     final int crossingLon =
  443.                             FastMath.max(0,
  444.                                          FastMath.min(tile.getLongitudeColumns() - 1,
  445.                                                       tile.getFloorLongitudeIndex(crossingGP.getLongitude())));

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

  451.                     if (inRange(crossingLatBefore, entryLat, exitLat)) {
  452.                         // look for intersection
  453.                         final NormalizedGeodeticPoint intersection;
  454.                         if (searchDomainSize(previousLat, previousLon, crossingLatBefore, crossingLon) <
  455.                             searchDomainSize(entryLat, entryLon, exitLat, exitLon)) {
  456.                             intersection = recurseIntersection(depth + 1, ellipsoid, position, los, tile,
  457.                                                                previousGP, previousLat, previousLon,
  458.                                                                crossingGP, crossingLatBefore, crossingLon);
  459.                         } else {
  460.                             intersection = noRecurseIntersection(ellipsoid, position, los, tile,
  461.                                                                  previousGP, previousLat, previousLon,
  462.                                                                  crossingLatBefore, crossingLon);
  463.                         }
  464.                         if (intersection != null) {
  465.                             return intersection;
  466.                         }
  467.                     }

  469.                     // prepare next segment
  470.                     previousGP  = crossingGP;
  471.                     previousLat = crossingLatAfter;
  472.                     previousLon = crossingLon;

  474.                 }

  476.             }
  477.         }

  479.         if (inRange(previousLat, entryLat, exitLat) && inRange(previousLon, entryLon, exitLon)) {
  480.             // last part of the segment, up to exit point
  481.             if (searchDomainSize(previousLat, previousLon, exitLat, exitLon) <
  482.                 searchDomainSize(entryLat, entryLon, exitLat, exitLon)) {
  483.                 return recurseIntersection(depth + 1, ellipsoid, position, los, tile,
  484.                                            previousGP, previousLat, previousLon,
  485.                                            exit, exitLat, exitLon);
  486.             } else {
  487.                 return noRecurseIntersection(ellipsoid, position, los, tile,
  488.                                              previousGP, previousLat, previousLon,
  489.                                              exitLat, exitLon);
  490.             }
  491.         } else {
  492.             return null;
  493.         }

  495.     }

  497.     /** Compute intersection of line with Digital Elevation Model in a sub-tile, without recursion.
  498.      * @param ellipsoid reference ellipsoid
  499.      * @param position pixel position in ellipsoid frame
  500.      * @param los pixel line-of-sight in ellipsoid frame
  501.      * @param tile Digital Elevation Model tile
  502.      * @param entry line-of-sight entry point in the sub-tile
  503.      * @param entryLat index to use for interpolating entry point elevation
  504.      * @param entryLon index to use for interpolating entry point elevation
  505.      * @param exitLat index to use for interpolating exit point elevation
  506.      * @param exitLon index to use for interpolating exit point elevation
  507.      * @return point at which the line first enters ground, or null if does not enter
  508.      * ground in the search sub-tile
  509.      */
  510.     private NormalizedGeodeticPoint noRecurseIntersection(final ExtendedEllipsoid ellipsoid,
  511.                                                           final Vector3D position, final Vector3D los,
  512.                                                           final MinMaxTreeTile tile,
  513.                                                           final NormalizedGeodeticPoint entry,
  514.                                                           final int entryLat, final int entryLon,
  515.                                                           final int exitLat, final int exitLon) {

  517.         NormalizedGeodeticPoint intersectionGP = null;
  518.         double intersectionDot = Double.POSITIVE_INFINITY;
  519.         for (int i = FastMath.min(entryLat, exitLat); i <= FastMath.max(entryLat, exitLat); ++i) {
  520.             for (int j = FastMath.min(entryLon, exitLon); j <= FastMath.max(entryLon, exitLon); ++j) {
  521.                 final NormalizedGeodeticPoint gp = tile.cellIntersection(entry, ellipsoid.convertLos(entry, los), i, j);
  522.                 if (gp != null) {

  524.                     // improve the point, by projecting it back on the 3D line, fixing the small body curvature at cell level
  525.                     final Vector3D delta = ellipsoid.transform(gp).subtract(position);
  526.                     final double   s     = Vector3D.dotProduct(delta, los) / los.getNormSq();
  527.                     if (s > 0) {
  528.                         final GeodeticPoint projected = ellipsoid.transform(new Vector3D(1, position, s, los),
  529.                                                                             ellipsoid.getBodyFrame(), null);
  530.                         final NormalizedGeodeticPoint normalizedProjected = new NormalizedGeodeticPoint(projected.getLatitude(),
  531.                                                                                                         projected.getLongitude(),
  532.                                                                                                         projected.getAltitude(),
  533.                                                                                                         gp.getLongitude());
  534.                         final NormalizedGeodeticPoint gpImproved = tile.cellIntersection(normalizedProjected,
  535.                                                                                          ellipsoid.convertLos(normalizedProjected, los),
  536.                                                                                          i, j);

  538.                         if (gpImproved != null) {
  539.                             final Vector3D point = ellipsoid.transform(gpImproved);
  540.                             final double dot = Vector3D.dotProduct(point.subtract(position), los);
  541.                             if (dot < intersectionDot) {
  542.                                 intersectionGP  = gpImproved;
  543.                                 intersectionDot = dot;
  544.                             }
  545.                         }
  546.                     }

  548.                 }
  549.             }
  550.         }

  552.         return intersectionGP;

  554.     }

  556.     /** Compute the size of a search domain.
  557.      * @param entryLat index to use for interpolating entry point elevation
  558.      * @param entryLon index to use for interpolating entry point elevation
  559.      * @param exitLat index to use for interpolating exit point elevation
  560.      * @param exitLon index to use for interpolating exit point elevation
  561.      * @return size of the search domain
  562.      */
  563.     private int searchDomainSize(final int entryLat, final int entryLon,
  564.                                  final int exitLat, final int exitLon) {
  565.         return (FastMath.abs(entryLat - exitLat) + 1) * (FastMath.abs(entryLon - exitLon) + 1);
  566.     }

  568.     /** Check if an index is inside a range.
  569.      * @param i index to check
  570.      * @param a first bound of the range (may be either below or above b)
  571.      * @param b second bound of the range (may be either below or above a)
  572.      * @return true if i is between a and b (inclusive)
  573.      */
  574.     private boolean inRange(final int i, final int a, final int b) {
  575.         return i >= FastMath.min(a, b) && i <= FastMath.max(a, b);
  576.     }

  578.     /** Compute a line-of-sight exit point from a tile.
  579.      * @param tile tile to consider
  580.      * @param ellipsoid reference ellipsoid
  581.      * @param position pixel position in ellipsoid frame
  582.      * @param los pixel line-of-sight in ellipsoid frame
  583.      * @return exit point
  584.      */
  585.     private LimitPoint findExit(final Tile tile, final ExtendedEllipsoid ellipsoid,
  586.                                 final Vector3D position, final Vector3D los) {

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

  593.         switch (tile.getLocation(exitGP.getLatitude(), exitGP.getLongitude())) {
  594.             case SOUTH_WEST :
  595.                 return new LimitPoint(ellipsoid, reference,
  596.                                       selectClosest(latitudeCrossing(ellipsoid, position,  los, tile.getMinimumLatitude(),  exitP),
  597.                                                     longitudeCrossing(ellipsoid, position, los, tile.getMinimumLongitude(), exitP),
  598.                                                     position),
  599.                                       true);
  600.             case WEST :
  601.                 return new LimitPoint(ellipsoid, reference,
  602.                                       longitudeCrossing(ellipsoid, position, los, tile.getMinimumLongitude(), exitP),
  603.                                       true);
  604.             case NORTH_WEST:
  605.                 return new LimitPoint(ellipsoid, reference,
  606.                                       selectClosest(latitudeCrossing(ellipsoid, position,  los, tile.getMaximumLatitude(),  exitP),
  607.                                                     longitudeCrossing(ellipsoid, position, los, tile.getMinimumLongitude(), exitP),
  608.                                                     position),
  609.                                       true);
  610.             case NORTH :
  611.                 return new LimitPoint(ellipsoid, reference,
  612.                                       latitudeCrossing(ellipsoid, position, los, tile.getMaximumLatitude(), exitP),
  613.                                       true);
  614.             case NORTH_EAST :
  615.                 return new LimitPoint(ellipsoid, reference,
  616.                                       selectClosest(latitudeCrossing(ellipsoid, position,  los, tile.getMaximumLatitude(),  exitP),
  617.                                                     longitudeCrossing(ellipsoid, position, los, tile.getMaximumLongitude(), exitP),
  618.                                                     position),
  619.                                       true);
  620.             case EAST :
  621.                 return new LimitPoint(ellipsoid, reference,
  622.                                       longitudeCrossing(ellipsoid, position, los, tile.getMaximumLongitude(), exitP),
  623.                                       true);
  624.             case SOUTH_EAST :
  625.                 return new LimitPoint(ellipsoid, reference,
  626.                                       selectClosest(latitudeCrossing(ellipsoid, position,  los, tile.getMinimumLatitude(),  exitP),
  627.                                                     longitudeCrossing(ellipsoid, position, los, tile.getMaximumLongitude(), exitP),
  628.                                                     position),
  629.                                       true);
  630.             case SOUTH :
  631.                 return new LimitPoint(ellipsoid, reference,
  632.                                       latitudeCrossing(ellipsoid, position, los, tile.getMinimumLatitude(), exitP),
  633.                                       true);
  634.             case HAS_INTERPOLATION_NEIGHBORS :
  635.                 return new LimitPoint(exitGP, false);

  637.             default :
  638.                 // this should never happen
  639.                 throw new RuggedInternalError(null);
  640.         }

  642.     }

  644.     /** Select point closest to line-of-sight start.
  645.      * @param p1 first point to consider
  646.      * @param p2 second point to consider
  647.      * @param position pixel position in ellipsoid frame
  648.      * @return either p1 or p2, depending on which is closest to position
  649.      */
  650.     private Vector3D selectClosest(final Vector3D p1, final Vector3D p2, final Vector3D position) {
  651.         return Vector3D.distance(p1, position) <= Vector3D.distance(p2, position) ? p1 : p2;
  652.     }

  654.     /** Get point at some latitude along a pixel line of sight.
  655.      * @param ellipsoid reference ellipsoid
  656.      * @param position pixel position (in body frame)
  657.      * @param los pixel line-of-sight, not necessarily normalized (in body frame)
  658.      * @param latitude latitude with respect to ellipsoid
  659.      * @param closeReference reference point used to select the closest solution
  660.      * when there are two points at the desired latitude along the line
  661.      * @return point at latitude, or closeReference if no such point can be found
  662.      */
  663.     private Vector3D latitudeCrossing(final ExtendedEllipsoid ellipsoid,
  664.                                       final Vector3D position, final Vector3D los,
  665.                                       final double latitude, final Vector3D closeReference) {
  666.         try {
  667.             return ellipsoid.pointAtLatitude(position, los, latitude, closeReference);
  668.         } catch (RuggedException re) {
  669.             return closeReference;
  670.         }
  671.     }

  673.     /** Get point at some latitude along a pixel line of sight.
  674.      * @param ellipsoid reference ellipsoid
  675.      * @param position pixel position (in body frame)
  676.      * @param los pixel line-of-sight, not necessarily normalized (in body frame)
  677.      * @param longitude longitude with respect to ellipsoid
  678.      * @param closeReference reference point used to select the closest solution
  679.      * when there are two points at the desired longitude along the line
  680.      * @return point at longitude, or closeReference if no such point can be found
  681.      */
  682.     private Vector3D longitudeCrossing(final ExtendedEllipsoid ellipsoid,
  683.                                        final Vector3D position, final Vector3D los,
  684.                                        final double longitude, final Vector3D closeReference) {
  685.         try {
  686.             return ellipsoid.pointAtLongitude(position, los, longitude);
  687.         } catch (RuggedException re) {
  688.             return closeReference;
  689.         }
  690.     }

  692.     /** Point at tile boundary. */
  693.     private static class LimitPoint {

  695.         /** Coordinates. */
  696.         private final NormalizedGeodeticPoint point;

  698.         /** Limit status. */
  699.         private final boolean side;

  701.         /** Simple constructor.
  702.          * @param ellipsoid reference ellipsoid
  703.          * @param referenceLongitude reference longitude lc such that the point longitude will
  704.          * be normalized between lc-π and lc+π
  705.          * @param cartesian Cartesian point
  706.          * @param side if true, the point is on a side limit, otherwise
  707.          * it is on a top/bottom limit
  708.          */
  709.         LimitPoint(final ExtendedEllipsoid ellipsoid, final double referenceLongitude,
  710.                    final Vector3D cartesian, final boolean side) {
  711.             this(ellipsoid.transform(cartesian, ellipsoid.getBodyFrame(), null, referenceLongitude), side);
  712.         }

  714.         /** Simple constructor.
  715.          * @param point coordinates
  716.          * @param side if true, the point is on a side limit, otherwise
  717.          * it is on a top/bottom limit
  718.          */
  719.         LimitPoint(final NormalizedGeodeticPoint point, final boolean side) {
  720.             this.point = point;
  721.             this.side  = side;
  722.         }

  724.         /** Get the point coordinates.
  725.          * @return point coordinates
  726.          */
  727.         public NormalizedGeodeticPoint getPoint() {
  728.             return point;
  729.         }

  731.         /** Check if point is on the side of a tile.
  732.          * @return true if the point is on a side limit, otherwise
  733.          * it is on a top/bottom limit
  734.          */
  735.         public boolean atSide() {
  736.             return side;
  737.         }

  739.     }

  741. }
Created with JaCoCo 0.8.2.201808211720