DuvenhageAlgorithm.java

  1. /* Copyright 2013-2025 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="https://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.      * @param isOverlappingTiles flag to tell if the DEM tiles are overlapping:
  75.      *                          true if overlapping; false otherwise.
  76.      */
  77.     public DuvenhageAlgorithm(final TileUpdater updater, final int maxCachedTiles,
  78.                               final boolean flatBody, final boolean isOverlappingTiles) {
  79.         this.cache = new TilesCache<MinMaxTreeTile>(new MinMaxTreeTileFactory(), updater,
  80.                                                     maxCachedTiles, isOverlappingTiles);
  81.         this.flatBody = flatBody;
  82.         this.algorithmId = flatBody ? AlgorithmId.DUVENHAGE_FLAT_BODY : AlgorithmId.DUVENHAGE;
  83.     }

  85.     /** {@inheritDoc} */
  86.     @Override
  87.     public NormalizedGeodeticPoint intersection(final ExtendedEllipsoid ellipsoid,
  88.                                                 final Vector3D position, final Vector3D los) {

  90.         DumpManager.dumpAlgorithm(this.algorithmId);

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

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

  98.         NormalizedGeodeticPoint current = null;
  99.         NormalizedGeodeticPoint positionGP = null;
  100.         double hMax = tile.getMaxElevation();
  101.         while (current == null) {

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

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

  126.                 if (current == null) {
  127.                     throw new RuggedException(RuggedMessages.DEM_ENTRY_POINT_IS_BEHIND_SPACECRAFT);
  128.                 }

  130.             } else {
  131.                 current = ellipsoid.transform(entryP, ellipsoid.getBodyFrame(), null, tile.getMinimumLongitude());
  132.             }

  134.             if (tile.getLocation(current.getLatitude(), current.getLongitude()) != Tile.Location.HAS_INTERPOLATION_NEIGHBORS) {
  135.                 // the entry point is in another tile
  136.                 tile    = cache.getTile(current.getLatitude(), current.getLongitude());
  137.                 hMax    = FastMath.max(hMax, tile.getMaxElevation());
  138.                 current = null;
  139.             }

  141.         }

  143.         // loop along the path
  144.         while (true) {

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

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

  166.             if (intersection != null) {
  167.                 // we have found the intersection
  168.                 return intersection;
  169.             } else if (exit.atSide()) {
  170.                 // no intersection on this tile, we can proceed to next part of the line-of-sight

  172.                 // select next tile after current point
  173.                 final Vector3D forward = new Vector3D(1.0, ellipsoid.transform(exit.getPoint()), STEP, los);
  174.                 current = ellipsoid.transform(forward, ellipsoid.getBodyFrame(), null, tile.getMinimumLongitude());
  175.                 tile = cache.getTile(current.getLatitude(), current.getLongitude());

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

  184.             } else {

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

  201.     /** {@inheritDoc} */
  202.     @Override
  203.     public NormalizedGeodeticPoint refineIntersection(final ExtendedEllipsoid ellipsoid,
  204.                                                       final Vector3D position, final Vector3D los,
  205.                                                       final NormalizedGeodeticPoint closeGuess) {

  207.         DumpManager.dumpAlgorithm(this.algorithmId);

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

  222.         } else {
  223.             // regular curved ellipsoid model

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

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

  244.                 final Vector3D                topoLOS           = ellipsoid.convertLos(normalizedProjected, los);
  245.                 final int                     iLat              = tile.getFloorLatitudeIndex(normalizedProjected.getLatitude());
  246.                 final int                     iLon              = tile.getFloorLongitudeIndex(normalizedProjected.getLongitude());
  247.                 final NormalizedGeodeticPoint foundIntersection = tile.cellIntersection(normalizedProjected, topoLOS, iLat, iLon);

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

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

  281.             }

  283.             // no intersection found
  284.             return null;

  286.         } // end test on flatbody

  288.     }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  413.                     // prepare next segment
  414.                     previousGP  = crossingGP;
  415.                     previousLat = crossingLat;
  416.                     previousLon = crossingLonAfter;

  418.                 }

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

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

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

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

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

  482.                     // prepare next segment
  483.                     previousGP  = crossingGP;
  484.                     previousLat = crossingLatAfter;
  485.                     previousLon = crossingLon;

  487.                 }

  489.             }
  490.         }

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

  508.     }

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

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

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

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

  561.                 }
  562.             }
  563.         }

  565.         return intersectionGP;

  567.     }

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

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

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

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

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

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

  655.     }

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

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

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

  705.     /** Point at tile boundary. */
  706.     private static class LimitPoint {

  708.         /** Coordinates. */
  709.         private final NormalizedGeodeticPoint point;

  711.         /** Limit status. */
  712.         private final boolean side;

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

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

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

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

  752.     }
  753. }
Created with JaCoCo 0.8.12.202403310830