BasicScanAlgorithm.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;

  19. import java.util.ArrayList;
  20. import java.util.List;

  22. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  23. import org.hipparchus.util.FastMath;
  24. import org.orekit.bodies.GeodeticPoint;
  25. import org.orekit.rugged.api.AlgorithmId;
  26. import org.orekit.rugged.errors.DumpManager;
  27. import org.orekit.rugged.raster.SimpleTile;
  28. import org.orekit.rugged.raster.SimpleTileFactory;
  29. import org.orekit.rugged.raster.Tile;
  30. import org.orekit.rugged.raster.TileUpdater;
  31. import org.orekit.rugged.raster.TilesCache;
  32. import org.orekit.rugged.utils.ExtendedEllipsoid;
  33. import org.orekit.rugged.utils.NormalizedGeodeticPoint;

  35. /** Intersection computation using a basic algorithm based on exhaustive scan.
  36.  * <p>
  37.  * The algorithm simply computes entry and exit points at high and low altitudes,
  38.  * and scans all Digital Elevation Models in the sub-tiles defined by these two
  39.  * corner points. It is not designed for operational use.
  40.  * </p>
  41.  * @author Luc Maisonobe
  42.  * @author Guylaine Prat
  43.  */
  44. public class BasicScanAlgorithm implements IntersectionAlgorithm {

  46.     /** Cache for DEM tiles. */
  47.     private final TilesCache<SimpleTile> cache;

  49.     /** Minimum altitude encountered. */
  50.     private double hMin;

  52.     /** Maximum altitude encountered. */
  53.     private double hMax;

  55.     /** Algorithm Id.
  56.      * @since 2.2 */
  57.     private final AlgorithmId algorithmId;

  59.     /** Simple constructor.
  60.      * @param updater updater used to load Digital Elevation Model tiles
  61.      * @param maxCachedTiles maximum number of tiles stored in the cache
  62.      */
  63.     public BasicScanAlgorithm(final TileUpdater updater, final int maxCachedTiles) {
  64.         this.cache = new TilesCache<>(new SimpleTileFactory(), updater, maxCachedTiles);
  65.         this.hMin  = Double.POSITIVE_INFINITY;
  66.         this.hMax  = Double.NEGATIVE_INFINITY;
  67.         this.algorithmId = AlgorithmId.BASIC_SLOW_EXHAUSTIVE_SCAN_FOR_TESTS_ONLY;
  68.     }

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

  75.         DumpManager.dumpAlgorithm(this.algorithmId);

  77.         // find the tiles between the entry and exit point in the Digital Elevation Model
  78.         NormalizedGeodeticPoint entryPoint = null;
  79.         NormalizedGeodeticPoint exitPoint  = null;
  80.         double minLatitude  = Double.NaN;
  81.         double maxLatitude  = Double.NaN;
  82.         double minLongitude = Double.NaN;
  83.         double maxLongitude = Double.NaN;
  84.         final List<SimpleTile> scannedTiles = new ArrayList<>();
  85.         double centralLongitude = Double.NaN;
  86.         for (boolean changedMinMax = true; changedMinMax; changedMinMax = checkMinMax(scannedTiles)) {

  88.             scannedTiles.clear();
  89.             // compute entry and exit points
  90.             entryPoint = ellipsoid.transform(ellipsoid.pointAtAltitude(position, los, Double.isInfinite(hMax) ? 0.0 : hMax),
  91.                     ellipsoid.getBodyFrame(), null,
  92.                     Double.isNaN(centralLongitude) ? 0.0 : centralLongitude);
  93.             final SimpleTile entryTile = cache.getTile(entryPoint.getLatitude(), entryPoint.getLongitude());
  94.             if (Double.isNaN(centralLongitude)) {
  95.                 centralLongitude = entryTile.getMinimumLongitude();
  96.                 entryPoint = new NormalizedGeodeticPoint(entryPoint.getLatitude(), entryPoint.getLongitude(),
  97.                         entryPoint.getAltitude(), centralLongitude);
  98.             }
  99.             addIfNotPresent(scannedTiles, entryTile);

  101.             exitPoint = ellipsoid.transform(ellipsoid.pointAtAltitude(position, los, Double.isInfinite(hMin) ? 0.0 : hMin),
  102.                     ellipsoid.getBodyFrame(), null, centralLongitude);
  103.             final SimpleTile exitTile = cache.getTile(exitPoint.getLatitude(), exitPoint.getLongitude());
  104.             addIfNotPresent(scannedTiles, exitTile);

  106.             minLatitude  = FastMath.min(entryPoint.getLatitude(),  exitPoint.getLatitude());
  107.             maxLatitude  = FastMath.max(entryPoint.getLatitude(),  exitPoint.getLatitude());
  108.             minLongitude = FastMath.min(entryPoint.getLongitude(), exitPoint.getLongitude());
  109.             maxLongitude = FastMath.max(entryPoint.getLongitude(), exitPoint.getLongitude());

  111.             if (scannedTiles.size() > 1) {
  112.                 // the entry and exit tiles are different, maybe other tiles should be added on the way
  113.                 // in the spirit of simple and exhaustive, we add all tiles in a rectangular area
  114.                 final double latStep = 0.5 * FastMath.min(entryTile.getLatitudeStep()  * entryTile.getLatitudeRows(),
  115.                         exitTile.getLatitudeStep()   * exitTile.getLatitudeRows());
  116.                 final double lonStep = 0.5 * FastMath.min(entryTile.getLongitudeStep() * entryTile.getLongitudeColumns(),
  117.                         exitTile.getLongitudeStep()  * exitTile.getLongitudeColumns());
  118.                 for (double latitude = minLatitude; latitude <= maxLatitude; latitude += latStep) {
  119.                     for (double longitude = minLongitude; longitude < maxLongitude; longitude += lonStep) {
  120.                         addIfNotPresent(scannedTiles, cache.getTile(latitude, longitude));
  121.                     }
  122.                 }
  123.             }

  125.         }

  127.         // scan the tiles
  128.         NormalizedGeodeticPoint intersectionGP = null;
  129.         double intersectionDot = Double.POSITIVE_INFINITY;
  130.         for (final SimpleTile tile : scannedTiles) {
  131.             for (int i = latitudeIndex(tile, minLatitude); i <= latitudeIndex(tile, maxLatitude); ++i) {
  132.                 for (int j = longitudeIndex(tile, minLongitude); j <= longitudeIndex(tile, maxLongitude); ++j) {
  133.                     final NormalizedGeodeticPoint gp = tile.cellIntersection(entryPoint, ellipsoid.convertLos(entryPoint, los), i, j);
  134.                     if (gp != null) {

  136.                         // improve the point, by projecting it back on the 3D line, fixing the small body curvature at cell level
  137.                         final Vector3D      delta     = ellipsoid.transform(gp).subtract(position);
  138.                         final double        s         = Vector3D.dotProduct(delta, los) / los.getNormSq();
  139.                         final GeodeticPoint projected = ellipsoid.transform(new Vector3D(1, position, s, los),
  140.                                 ellipsoid.getBodyFrame(), null);
  141.                         final NormalizedGeodeticPoint normalizedProjected = new NormalizedGeodeticPoint(projected.getLatitude(),
  142.                                 projected.getLongitude(),
  143.                                 projected.getAltitude(),
  144.                                 gp.getLongitude());
  145.                         final NormalizedGeodeticPoint gpImproved = tile.cellIntersection(normalizedProjected,
  146.                                 ellipsoid.convertLos(normalizedProjected, los),
  147.                                 i, j);

  149.                         if (gpImproved != null) {
  150.                             final Vector3D point = ellipsoid.transform(gpImproved);
  151.                             final double dot = Vector3D.dotProduct(point.subtract(position), los);
  152.                             if (dot < intersectionDot) {
  153.                                 intersectionGP  = gpImproved;
  154.                                 intersectionDot = dot;
  155.                             }
  156.                         }

  158.                     }
  159.                 }
  160.             }
  161.         }

  163.         return intersectionGP;
  164.     }

  166.     /** {@inheritDoc} */
  167.     @Override
  168.     public NormalizedGeodeticPoint refineIntersection(final ExtendedEllipsoid ellipsoid,
  169.                                                       final Vector3D position, final Vector3D los,
  170.                                                       final NormalizedGeodeticPoint closeGuess) {
  171.         DumpManager.dumpAlgorithm(this.algorithmId);
  172.         final Vector3D      delta     = ellipsoid.transform(closeGuess).subtract(position);
  173.         final double        s         = Vector3D.dotProduct(delta, los) / los.getNormSq();
  174.         final GeodeticPoint projected = ellipsoid.transform(new Vector3D(1, position, s, los),
  175.                 ellipsoid.getBodyFrame(), null);
  176.         final NormalizedGeodeticPoint normalizedProjected = new NormalizedGeodeticPoint(projected.getLatitude(),
  177.                 projected.getLongitude(),
  178.                 projected.getAltitude(),
  179.                 closeGuess.getLongitude());
  180.         final Tile          tile      = cache.getTile(normalizedProjected.getLatitude(),
  181.                 normalizedProjected.getLongitude());
  182.         return tile.cellIntersection(normalizedProjected,
  183.                 ellipsoid.convertLos(normalizedProjected, los),
  184.                 tile.getFloorLatitudeIndex(normalizedProjected.getLatitude()),
  185.                 tile.getFloorLongitudeIndex(normalizedProjected.getLongitude()));
  186.     }

  188.     /** {@inheritDoc} */
  189.     @Override
  190.     public double getElevation(final double latitude, final double longitude) {
  191.         DumpManager.dumpAlgorithm(this.algorithmId);
  192.         final Tile tile = cache.getTile(latitude, longitude);
  193.         return tile.interpolateElevation(latitude, longitude);
  194.     }

  196.     /** {@inheritDoc} */
  197.     @Override
  198.     public AlgorithmId getAlgorithmId() {
  199.         return this.algorithmId;
  200.     }

  202.     /** Check the overall min and max altitudes.
  203.      * @param tiles tiles to check
  204.      * @return true if the tile changed either min or max altitude
  205.      */
  206.     private boolean checkMinMax(final List<SimpleTile> tiles) {

  208.         boolean changedMinMax = false;

  210.         for (final SimpleTile tile : tiles) {

  212.             // check minimum altitude
  213.             if (tile.getMinElevation() < hMin) {
  214.                 hMin          = tile.getMinElevation();
  215.                 changedMinMax = true;
  216.             }

  218.             // check maximum altitude
  219.             if (tile.getMaxElevation() > hMax) {
  220.                 hMax          = tile.getMaxElevation();
  221.                 changedMinMax = true;
  222.             }

  224.         }

  226.         return changedMinMax;

  228.     }

  230.     /** Add a tile to a list if not already present.
  231.      * @param list tiles list
  232.      * @param tile new tile to consider
  233.      */
  234.     private void addIfNotPresent(final List<SimpleTile> list, final SimpleTile tile) {

  236.         // look for existing tiles in the list
  237.         for (final SimpleTile existing : list) {
  238.             if (existing == tile) {
  239.                 return;
  240.             }
  241.         }

  243.         // the tile was not there, add it
  244.         list.add(tile);

  246.     }

  248.     /** Get latitude index.
  249.      * @param tile current tile
  250.      * @param latitude current latitude
  251.      * @return index of latitude, truncated at tiles limits
  252.      */
  253.     private int latitudeIndex(final SimpleTile tile, final double latitude) {
  254.         final int rawIndex = tile.getFloorLatitudeIndex(latitude);
  255.         return FastMath.min(FastMath.max(0, rawIndex), tile.getLatitudeRows());
  256.     }

  258.     /** Get longitude index.
  259.      * @param tile current tile
  260.      * @param longitude current longitude
  261.      * @return index of longitude, truncated at tiles limits
  262.      */
  263.     private int longitudeIndex(final SimpleTile tile, final double longitude) {
  264.         final int rawIndex = tile.getFloorLongitudeIndex(longitude);
  265.         return FastMath.min(FastMath.max(0, rawIndex), tile.getLongitudeColumns());
  266.     }
  267. }
Created with JaCoCo 0.8.7.202105040129