DuvenhageAlgorithm.java

  1. /* Copyright 2013-2022 CS GROUP
  2.  * Licensed to CS GROUP (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.     /** Algorithm Id.
  62.      * @since 2.2 */
  63.     private final AlgorithmId algorithmId;

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

  82.     /** {@inheritDoc} */
  83.     @Override
  84.     public NormalizedGeodeticPoint intersection(final ExtendedEllipsoid ellipsoid,
  85.                                                 final Vector3D position, final Vector3D los) {

  87.         DumpManager.dumpAlgorithm(this.algorithmId);

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

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

  95.         NormalizedGeodeticPoint current = null;
  96.         double hMax = tile.getMaxElevation();
  97.         while (current == null) {

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

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

  121.                 if (current == null) {
  122.                     throw new RuggedException(RuggedMessages.DEM_ENTRY_POINT_IS_BEHIND_SPACECRAFT);
  123.                 }

  125.             } else {
  126.                 current = ellipsoid.transform(entryP, ellipsoid.getBodyFrame(), null, tile.getMinimumLongitude());
  127.             }

  129.             if (tile.getLocation(current.getLatitude(), current.getLongitude()) != Tile.Location.HAS_INTERPOLATION_NEIGHBORS) {
  130.                 // the entry point is in another tile
  131.                 tile    = cache.getTile(current.getLatitude(), current.getLongitude());
  132.                 hMax    = FastMath.max(hMax, tile.getMaxElevation());
  133.                 current = null;
  134.             }

  136.         }

  138.         // loop along the path
  139.         while (true) {

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

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

  161.             if (intersection != null) {
  162.                 // we have found the intersection
  163.                 return intersection;
  164.             } else if (exit.atSide()) {
  165.                 // no intersection on this tile, we can proceed to next part of the line-of-sight

  167.                 // select next tile after current point
  168.                 final Vector3D forward = new Vector3D(1.0, ellipsoid.transform(exit.getPoint()), STEP, los);
  169.                 current = ellipsoid.transform(forward, ellipsoid.getBodyFrame(), null, tile.getMinimumLongitude());
  170.                 tile = cache.getTile(current.getLatitude(), current.getLongitude());

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

  179.             } else {

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

  196.     /** {@inheritDoc} */
  197.     @Override
  198.     public NormalizedGeodeticPoint refineIntersection(final ExtendedEllipsoid ellipsoid,
  199.                                                       final Vector3D position, final Vector3D los,
  200.                                                       final NormalizedGeodeticPoint closeGuess) {

  202.         DumpManager.dumpAlgorithm(this.algorithmId);

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

  217.         } else {
  218.             // regular curved ellipsoid model

  220.             NormalizedGeodeticPoint currentGuess = closeGuess;

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

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

  240.                 final Vector3D                topoLOS           = ellipsoid.convertLos(normalizedProjected, los);
  241.                 final int                     iLat              = tile.getFloorLatitudeIndex(normalizedProjected.getLatitude());
  242.                 final int                     iLon              = tile.getFloorLongitudeIndex(normalizedProjected.getLongitude());
  243.                 final NormalizedGeodeticPoint foundIntersection = tile.cellIntersection(normalizedProjected, topoLOS, iLat, iLon);

  245.                 if (foundIntersection != null) {
  246.                     // nominal case, we were able to refine the intersection
  247.                     return foundIntersection;
  248.                 } else {
  249.                     // extremely rare case: we have lost the intersection

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


  271.                     // We recompute fully a new guess, starting from the point after current cell
  272.                     final GeodeticPoint currentGuessGP = intersection(ellipsoid, cellExit, los);
  273.                     currentGuess = new NormalizedGeodeticPoint(currentGuessGP.getLatitude(),
  274.                                                                currentGuessGP.getLongitude(),
  275.                                                                currentGuessGP.getAltitude(),
  276.                                                                projected.getLongitude());
  277.                 }

  279.             }

  281.             // no intersection found
  282.             return null;

  284.         } // end test on flatbody

  286.     }

  288.     /** {@inheritDoc} */
  289.     @Override
  290.     public double getElevation(final double latitude, final double longitude) {

  292.         DumpManager.dumpAlgorithm(this.algorithmId);
  293.         final Tile tile = cache.getTile(latitude, longitude);
  294.         return tile.interpolateElevation(latitude, longitude);
  295.     }

  297.     /** {@inheritDoc} */
  298.     @Override
  299.     public AlgorithmId getAlgorithmId() {
  300.         return this.algorithmId;
  301.     }

  303.     /** Compute intersection of line with Digital Elevation Model in a sub-tile.
  304.      * @param depth recursion depth
  305.      * @param ellipsoid reference ellipsoid
  306.      * @param position pixel position in ellipsoid frame
  307.      * @param los pixel line-of-sight in ellipsoid frame
  308.      * @param tile Digital Elevation Model tile
  309.      * @param entry line-of-sight entry point in the sub-tile
  310.      * @param entryLat index to use for interpolating entry point elevation
  311.      * @param entryLon index to use for interpolating entry point elevation
  312.      * @param exit line-of-sight exit point from the sub-tile
  313.      * @param exitLat index to use for interpolating exit point elevation
  314.      * @param exitLon index to use for interpolating exit point elevation
  315.      * @return point at which the line first enters ground, or null if does not enter
  316.      * ground in the search sub-tile
  317.      */
  318.     private NormalizedGeodeticPoint recurseIntersection(final int depth, final ExtendedEllipsoid ellipsoid,
  319.                                                         final Vector3D position, final Vector3D los,
  320.                                                         final MinMaxTreeTile tile,
  321.                                                         final NormalizedGeodeticPoint entry, final int entryLat, final int entryLon,
  322.                                                         final NormalizedGeodeticPoint exit, final int exitLat, final int exitLon) {

  324.         if (depth > 30) {
  325.             // this should never happen
  326.             throw new RuggedInternalError(null);
  327.         }

  329.         if (searchDomainSize(entryLat, entryLon, exitLat, exitLon) < 4) {
  330.             // we have narrowed the search down to a few cells
  331.             return noRecurseIntersection(ellipsoid, position, los, tile,
  332.                                          entry, entryLat, entryLon, exitLat, exitLon);
  333.         }

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

  343.         NormalizedGeodeticPoint previousGP    = entry;
  344.         int                     previousLat   = entryLat;
  345.         int                     previousLon   = entryLon;
  346.         final double            angularMargin = STEP / ellipsoid.getEquatorialRadius();

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

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

  356.                 // compute segment endpoints
  357.                 final double longitude = tile.getLongitudeAtIndex(crossingLon);
  358.                 if (longitude >= FastMath.min(entry.getLongitude(), exit.getLongitude()) - angularMargin &&
  359.                     longitude <= FastMath.max(entry.getLongitude(), exit.getLongitude()) + angularMargin) {

  361.                     NormalizedGeodeticPoint crossingGP = null;
  362.                     if (!flatBody) {
  363.                         try {
  364.                             // full computation of crossing point
  365.                             final Vector3D crossingP = ellipsoid.pointAtLongitude(position, los, longitude);
  366.                             crossingGP = ellipsoid.transform(crossingP, ellipsoid.getBodyFrame(), null,
  367.                                                              tile.getMinimumLongitude());
  368.                         } catch (RuggedException re) {
  369.                             // in some very rare cases of numerical noise, we miss the crossing point
  370.                             crossingGP = null;
  371.                         }
  372.                     }
  373.                     if (crossingGP == null) {
  374.                         // linear approximation of crossing point
  375.                         final double d  = exit.getLongitude() - entry.getLongitude();
  376.                         final double cN = (exit.getLongitude() - longitude) / d;
  377.                         final double cX = (longitude - entry.getLongitude()) / d;
  378.                         crossingGP = new NormalizedGeodeticPoint(cN * entry.getLatitude() + cX * exit.getLatitude(),
  379.                                                                  longitude,
  380.                                                                  cN * entry.getAltitude() + cX * exit.getAltitude(),
  381.                                                                  tile.getMinimumLongitude());
  382.                     }
  383.                     final int crossingLat =
  384.                             FastMath.max(0,
  385.                                          FastMath.min(tile.getLatitudeRows() - 1,
  386.                                                       tile.getFloorLatitudeIndex(crossingGP.getLatitude())));

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

  392.                     if (inRange(crossingLonBefore, entryLon, exitLon)) {
  393.                         // look for intersection
  394.                         final NormalizedGeodeticPoint intersection;
  395.                         if (searchDomainSize(previousLat, previousLon, crossingLat, crossingLonBefore) <
  396.                             searchDomainSize(entryLat, entryLon, exitLat, exitLon)) {
  397.                             intersection = recurseIntersection(depth + 1, ellipsoid, position, los, tile,
  398.                                                                previousGP, previousLat, previousLon,
  399.                                                                crossingGP, crossingLat, crossingLonBefore);
  400.                         } else {
  401.                             // we failed to reduce domain size, probably due to numerical problems
  402.                             intersection = noRecurseIntersection(ellipsoid, position, los, tile,
  403.                                                                  previousGP, previousLat, previousLon,
  404.                                                                  crossingLat, crossingLonBefore);
  405.                         }
  406.                         if (intersection != null) {
  407.                             return intersection;
  408.                         }
  409.                     }

  411.                     // prepare next segment
  412.                     previousGP  = crossingGP;
  413.                     previousLat = crossingLat;
  414.                     previousLon = crossingLonAfter;

  416.                 }

  418.             }
  419.         } else {
  420.             // recurse through latitude crossings
  421.             final int[] crossings = tile.getCrossedBoundaryRows(previousLat, exitLat, level + 1);
  422.             for (final int crossingLat : crossings) {

  424.                 // compute segment endpoints
  425.                 final double latitude = tile.getLatitudeAtIndex(crossingLat);
  426.                 if (latitude >= FastMath.min(entry.getLatitude(), exit.getLatitude()) - angularMargin &&
  427.                     latitude <= FastMath.max(entry.getLatitude(), exit.getLatitude()) + angularMargin) {

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

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

  462.                     if (inRange(crossingLatBefore, entryLat, exitLat)) {
  463.                         // look for intersection
  464.                         final NormalizedGeodeticPoint intersection;
  465.                         if (searchDomainSize(previousLat, previousLon, crossingLatBefore, crossingLon) <
  466.                             searchDomainSize(entryLat, entryLon, exitLat, exitLon)) {
  467.                             intersection = recurseIntersection(depth + 1, ellipsoid, position, los, tile,
  468.                                                                previousGP, previousLat, previousLon,
  469.                                                                crossingGP, crossingLatBefore, crossingLon);
  470.                         } else {
  471.                             intersection = noRecurseIntersection(ellipsoid, position, los, tile,
  472.                                                                  previousGP, previousLat, previousLon,
  473.                                                                  crossingLatBefore, crossingLon);
  474.                         }
  475.                         if (intersection != null) {
  476.                             return intersection;
  477.                         }
  478.                     }

  480.                     // prepare next segment
  481.                     previousGP  = crossingGP;
  482.                     previousLat = crossingLatAfter;
  483.                     previousLon = crossingLon;

  485.                 }

  487.             }
  488.         }

  490.         if (inRange(previousLat, entryLat, exitLat) && inRange(previousLon, entryLon, exitLon)) {
  491.             // last part of the segment, up to exit point
  492.             if (searchDomainSize(previousLat, previousLon, exitLat, exitLon) <
  493.                 searchDomainSize(entryLat, entryLon, exitLat, exitLon)) {
  494.                 return recurseIntersection(depth + 1, ellipsoid, position, los, tile,
  495.                                            previousGP, previousLat, previousLon,
  496.                                            exit, exitLat, exitLon);
  497.             } else {
  498.                 return noRecurseIntersection(ellipsoid, position, los, tile,
  499.                                              previousGP, previousLat, previousLon,
  500.                                              exitLat, exitLon);
  501.             }
  502.         } else {
  503.             return null;
  504.         }

  506.     }

  508.     /** Compute intersection of line with Digital Elevation Model in a sub-tile, without recursion.
  509.      * @param ellipsoid reference ellipsoid
  510.      * @param position pixel position in ellipsoid frame
  511.      * @param los pixel line-of-sight in ellipsoid frame
  512.      * @param tile Digital Elevation Model tile
  513.      * @param entry line-of-sight entry point in the sub-tile
  514.      * @param entryLat index to use for interpolating entry point elevation
  515.      * @param entryLon index to use for interpolating entry point elevation
  516.      * @param exitLat index to use for interpolating exit point elevation
  517.      * @param exitLon index to use for interpolating exit point elevation
  518.      * @return point at which the line first enters ground, or null if does not enter
  519.      * ground in the search sub-tile
  520.      */
  521.     private NormalizedGeodeticPoint noRecurseIntersection(final ExtendedEllipsoid ellipsoid,
  522.                                                           final Vector3D position, final Vector3D los,
  523.                                                           final MinMaxTreeTile tile,
  524.                                                           final NormalizedGeodeticPoint entry,
  525.                                                           final int entryLat, final int entryLon,
  526.                                                           final int exitLat, final int exitLon) {

  528.         NormalizedGeodeticPoint intersectionGP = null;
  529.         double intersectionDot = Double.POSITIVE_INFINITY;
  530.         for (int i = FastMath.min(entryLat, exitLat); i <= FastMath.max(entryLat, exitLat); ++i) {
  531.             for (int j = FastMath.min(entryLon, exitLon); j <= FastMath.max(entryLon, exitLon); ++j) {
  532.                 final NormalizedGeodeticPoint gp = tile.cellIntersection(entry, ellipsoid.convertLos(entry, los), i, j);
  533.                 if (gp != null) {

  535.                     // improve the point, by projecting it back on the 3D line, fixing the small body curvature at cell level
  536.                     final Vector3D delta = ellipsoid.transform(gp).subtract(position);
  537.                     final double   s     = Vector3D.dotProduct(delta, los) / los.getNormSq();
  538.                     if (s > 0) {
  539.                         final GeodeticPoint projected = ellipsoid.transform(new Vector3D(1, position, s, los),
  540.                                                                             ellipsoid.getBodyFrame(), null);
  541.                         final NormalizedGeodeticPoint normalizedProjected = new NormalizedGeodeticPoint(projected.getLatitude(),
  542.                                                                                                         projected.getLongitude(),
  543.                                                                                                         projected.getAltitude(),
  544.                                                                                                         gp.getLongitude());
  545.                         final NormalizedGeodeticPoint gpImproved = tile.cellIntersection(normalizedProjected,
  546.                                                                                          ellipsoid.convertLos(normalizedProjected, los),
  547.                                                                                          i, j);

  549.                         if (gpImproved != null) {
  550.                             final Vector3D point = ellipsoid.transform(gpImproved);
  551.                             final double dot = Vector3D.dotProduct(point.subtract(position), los);
  552.                             if (dot < intersectionDot) {
  553.                                 intersectionGP  = gpImproved;
  554.                                 intersectionDot = dot;
  555.                             }
  556.                         }
  557.                     }

  559.                 }
  560.             }
  561.         }

  563.         return intersectionGP;

  565.     }

  567.     /** Compute the size of a search domain.
  568.      * @param entryLat index to use for interpolating entry point elevation
  569.      * @param entryLon index to use for interpolating entry point elevation
  570.      * @param exitLat index to use for interpolating exit point elevation
  571.      * @param exitLon index to use for interpolating exit point elevation
  572.      * @return size of the search domain
  573.      */
  574.     private int searchDomainSize(final int entryLat, final int entryLon,
  575.                                  final int exitLat, final int exitLon) {
  576.         return (FastMath.abs(entryLat - exitLat) + 1) * (FastMath.abs(entryLon - exitLon) + 1);
  577.     }

  579.     /** Check if an index is inside a range.
  580.      * @param i index to check
  581.      * @param a first bound of the range (may be either below or above b)
  582.      * @param b second bound of the range (may be either below or above a)
  583.      * @return true if i is between a and b (inclusive)
  584.      */
  585.     private boolean inRange(final int i, final int a, final int b) {
  586.         return i >= FastMath.min(a, b) && i <= FastMath.max(a, b);
  587.     }

  589.     /** Compute a line-of-sight exit point from a tile.
  590.      * @param tile tile to consider
  591.      * @param ellipsoid reference ellipsoid
  592.      * @param position pixel position in ellipsoid frame
  593.      * @param los pixel line-of-sight in ellipsoid frame
  594.      * @return exit point
  595.      */
  596.     private LimitPoint findExit(final Tile tile, final ExtendedEllipsoid ellipsoid,
  597.                                 final Vector3D position, final Vector3D los) {

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

  604.         switch (tile.getLocation(exitGP.getLatitude(), exitGP.getLongitude())) {
  605.             case SOUTH_WEST :
  606.                 return new LimitPoint(ellipsoid, reference,
  607.                                       selectClosest(latitudeCrossing(ellipsoid, position,  los, tile.getMinimumLatitude(),  exitP),
  608.                                                     longitudeCrossing(ellipsoid, position, los, tile.getMinimumLongitude(), exitP),
  609.                                                     position),
  610.                                       true);
  611.             case WEST :
  612.                 return new LimitPoint(ellipsoid, reference,
  613.                                       longitudeCrossing(ellipsoid, position, los, tile.getMinimumLongitude(), exitP),
  614.                                       true);
  615.             case NORTH_WEST:
  616.                 return new LimitPoint(ellipsoid, reference,
  617.                                       selectClosest(latitudeCrossing(ellipsoid, position,  los, tile.getMaximumLatitude(),  exitP),
  618.                                                     longitudeCrossing(ellipsoid, position, los, tile.getMinimumLongitude(), exitP),
  619.                                                     position),
  620.                                       true);
  621.             case NORTH :
  622.                 return new LimitPoint(ellipsoid, reference,
  623.                                       latitudeCrossing(ellipsoid, position, los, tile.getMaximumLatitude(), exitP),
  624.                                       true);
  625.             case NORTH_EAST :
  626.                 return new LimitPoint(ellipsoid, reference,
  627.                                       selectClosest(latitudeCrossing(ellipsoid, position,  los, tile.getMaximumLatitude(),  exitP),
  628.                                                     longitudeCrossing(ellipsoid, position, los, tile.getMaximumLongitude(), exitP),
  629.                                                     position),
  630.                                       true);
  631.             case EAST :
  632.                 return new LimitPoint(ellipsoid, reference,
  633.                                       longitudeCrossing(ellipsoid, position, los, tile.getMaximumLongitude(), exitP),
  634.                                       true);
  635.             case SOUTH_EAST :
  636.                 return new LimitPoint(ellipsoid, reference,
  637.                                       selectClosest(latitudeCrossing(ellipsoid, position,  los, tile.getMinimumLatitude(),  exitP),
  638.                                                     longitudeCrossing(ellipsoid, position, los, tile.getMaximumLongitude(), exitP),
  639.                                                     position),
  640.                                       true);
  641.             case SOUTH :
  642.                 return new LimitPoint(ellipsoid, reference,
  643.                                       latitudeCrossing(ellipsoid, position, los, tile.getMinimumLatitude(), exitP),
  644.                                       true);
  645.             case HAS_INTERPOLATION_NEIGHBORS :
  646.                 return new LimitPoint(exitGP, false);

  648.             default :
  649.                 // this should never happen
  650.                 throw new RuggedInternalError(null);
  651.         }

  653.     }

  655.     /** Select point closest to line-of-sight start.
  656.      * @param p1 first point to consider
  657.      * @param p2 second point to consider
  658.      * @param position pixel position in ellipsoid frame
  659.      * @return either p1 or p2, depending on which is closest to position
  660.      */
  661.     private Vector3D selectClosest(final Vector3D p1, final Vector3D p2, final Vector3D position) {
  662.         return Vector3D.distance(p1, position) <= Vector3D.distance(p2, position) ? p1 : p2;
  663.     }

  665.     /** Get point at some latitude along a pixel line of sight.
  666.      * @param ellipsoid reference ellipsoid
  667.      * @param position pixel position (in body frame)
  668.      * @param los pixel line-of-sight, not necessarily normalized (in body frame)
  669.      * @param latitude latitude with respect to ellipsoid
  670.      * @param closeReference reference point used to select the closest solution
  671.      * when there are two points at the desired latitude along the line
  672.      * @return point at latitude, or closeReference if no such point can be found
  673.      */
  674.     private Vector3D latitudeCrossing(final ExtendedEllipsoid ellipsoid,
  675.                                       final Vector3D position, final Vector3D los,
  676.                                       final double latitude, final Vector3D closeReference) {
  677.         try {
  678.             return ellipsoid.pointAtLatitude(position, los, latitude, closeReference);
  679.         } catch (RuggedException re) {
  680.             return closeReference;
  681.         }
  682.     }

  684.     /** Get point at some latitude along a pixel line of sight.
  685.      * @param ellipsoid reference ellipsoid
  686.      * @param position pixel position (in body frame)
  687.      * @param los pixel line-of-sight, not necessarily normalized (in body frame)
  688.      * @param longitude longitude with respect to ellipsoid
  689.      * @param closeReference reference point used to select the closest solution
  690.      * when there are two points at the desired longitude along the line
  691.      * @return point at longitude, or closeReference if no such point can be found
  692.      */
  693.     private Vector3D longitudeCrossing(final ExtendedEllipsoid ellipsoid,
  694.                                        final Vector3D position, final Vector3D los,
  695.                                        final double longitude, final Vector3D closeReference) {
  696.         try {
  697.             return ellipsoid.pointAtLongitude(position, los, longitude);
  698.         } catch (RuggedException re) {
  699.             return closeReference;
  700.         }
  701.     }

  703.     /** Point at tile boundary. */
  704.     private static class LimitPoint {

  706.         /** Coordinates. */
  707.         private final NormalizedGeodeticPoint point;

  709.         /** Limit status. */
  710.         private final boolean side;

  712.         /** Simple constructor.
  713.          * @param ellipsoid reference ellipsoid
  714.          * @param referenceLongitude reference longitude lc such that the point longitude will
  715.          * be normalized between lc-π and lc+π
  716.          * @param cartesian Cartesian point
  717.          * @param side if true, the point is on a side limit, otherwise
  718.          * it is on a top/bottom limit
  719.          */
  720.         LimitPoint(final ExtendedEllipsoid ellipsoid, final double referenceLongitude,
  721.                    final Vector3D cartesian, final boolean side) {
  722.             this(ellipsoid.transform(cartesian, ellipsoid.getBodyFrame(), null, referenceLongitude), side);
  723.         }

  725.         /** Simple constructor.
  726.          * @param point coordinates
  727.          * @param side if true, the point is on a side limit, otherwise
  728.          * it is on a top/bottom limit
  729.          */
  730.         LimitPoint(final NormalizedGeodeticPoint point, final boolean side) {
  731.             this.point = point;
  732.             this.side  = side;
  733.         }

  735.         /** Get the point coordinates.
  736.          * @return point coordinates
  737.          */
  738.         public NormalizedGeodeticPoint getPoint() {
  739.             return point;
  740.         }

  742.         /** Check if point is on the side of a tile.
  743.          * @return true if the point is on a side limit, otherwise
  744.          * it is on a top/bottom limit
  745.          */
  746.         public boolean atSide() {
  747.             return side;
  748.         }

  750.     }
  751. }
Created with JaCoCo 0.8.7.202105040129