SimpleTile.java

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

  18. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  19. import org.hipparchus.util.FastMath;
  20. import org.hipparchus.util.Precision;
  21. import java.util.Arrays;

  22. import org.orekit.bodies.GeodeticPoint;
  23. import org.orekit.rugged.errors.DumpManager;
  24. import org.orekit.rugged.errors.RuggedException;
  25. import org.orekit.rugged.errors.RuggedMessages;
  26. import org.orekit.rugged.utils.MaxSelector;
  27. import org.orekit.rugged.utils.MinSelector;
  28. import org.orekit.rugged.utils.NormalizedGeodeticPoint;


  29. /** Simple implementation of a {@link Tile}.
  30.  * @see SimpleTileFactory
  31.  * @author Luc Maisonobe
  32.  * @author Guylaine Prat
  33.  */
  34. public class SimpleTile implements Tile {

  35.     /** Tolerance used to interpolate points slightly out of tile (in cells). */
  36.     private static final double TOLERANCE = 1.0 / 8.0;

  37.     /** Minimum latitude. */
  38.     private double minLatitude;

  39.     /** Minimum longitude. */
  40.     private double minLongitude;

  41.     /** Step in latitude (size of one raster element). */
  42.     private double latitudeStep;

  43.     /** Step in longitude (size of one raster element). */
  44.     private double longitudeStep;

  45.     /** Number of latitude rows. */
  46.     private int latitudeRows;

  47.     /** Number of longitude columns. */
  48.     private int longitudeColumns;

  49.     /** Minimum elevation. */
  50.     private double minElevation;

  51.     /** Latitude index of min elevation. */
  52.     private int minElevationLatitudeIndex;

  53.     /** Longitude index of min elevation. */
  54.     private int minElevationLongitudeIndex;

  55.     /** Maximum elevation. */
  56.     private double maxElevation;

  57.     /** Latitude index of max elevation. */
  58.     private int maxElevationLatitudeIndex;

  59.     /** Longitude index of max elevation. */
  60.     private int maxElevationLongitudeIndex;

  61.     /** Elevation array. */
  62.     private double[] elevations;

  63.     /** Simple constructor.
  64.      * <p>
  65.      * Creates an empty tile.
  66.      * </p>
  67.      */
  68.     protected SimpleTile() {
  69.     }

  70.     /** {@inheritDoc} */
  71.     @Override
  72.     public void setGeometry(final double newMinLatitude, final double newMinLongitude,
  73.                             final double newLatitudeStep, final double newLongitudeStep,
  74.                             final int newLatitudeRows, final int newLongitudeColumns) {
  75.         this.minLatitude                = newMinLatitude;
  76.         this.minLongitude               = newMinLongitude;
  77.         this.latitudeStep               = newLatitudeStep;
  78.         this.longitudeStep              = newLongitudeStep;
  79.         this.latitudeRows               = newLatitudeRows;
  80.         this.longitudeColumns           = newLongitudeColumns;
  81.         this.minElevation               = Double.POSITIVE_INFINITY;
  82.         this.minElevationLatitudeIndex  = -1;
  83.         this.minElevationLongitudeIndex = -1;
  84.         this.maxElevation               = Double.NEGATIVE_INFINITY;
  85.         this.maxElevationLatitudeIndex  = -1;
  86.         this.maxElevationLongitudeIndex = -1;

  87.         if (newLatitudeRows < 1 || newLongitudeColumns < 1) {
  88.             throw new RuggedException(RuggedMessages.EMPTY_TILE, newLatitudeRows, newLongitudeColumns);
  89.         }
  90.         this.elevations = new double[newLatitudeRows * newLongitudeColumns];
  91.         Arrays.fill(elevations, Double.NaN);

  92.     }

  93.     /** {@inheritDoc} */
  94.     @Override
  95.     public void tileUpdateCompleted() {
  96.         processUpdatedElevation(elevations);
  97.     }

  98.     /** Process elevation array at completion.
  99.      * <p>
  100.      * This method is called at tile update completion, it is
  101.      * expected to be overridden by subclasses. The default
  102.      * implementation does nothing.
  103.      * </p>
  104.      * @param elevationsArray elevations array
  105.      */
  106.     protected void processUpdatedElevation(final double[] elevationsArray) {
  107.         // do nothing by default
  108.     }

  109.     /** {@inheritDoc} */
  110.     @Override
  111.     public double getMinimumLatitude() {
  112.         return getLatitudeAtIndex(0);
  113.     }

  114.     /** {@inheritDoc} */
  115.     @Override
  116.     public double getLatitudeAtIndex(final int latitudeIndex) {
  117.         return minLatitude + latitudeStep * latitudeIndex;
  118.     }

  119.     /** {@inheritDoc} */
  120.     @Override
  121.     public double getMaximumLatitude() {
  122.         return getLatitudeAtIndex(latitudeRows - 1);
  123.     }

  124.     /** {@inheritDoc} */
  125.     @Override
  126.     public double getMinimumLongitude() {
  127.         return getLongitudeAtIndex(0);
  128.     }

  129.     /** {@inheritDoc} */
  130.     @Override
  131.     public double getLongitudeAtIndex(final int longitudeIndex) {
  132.         return minLongitude + longitudeStep * longitudeIndex;
  133.     }

  134.     /** {@inheritDoc} */
  135.     @Override
  136.     public double getMaximumLongitude() {
  137.         return getLongitudeAtIndex(longitudeColumns - 1);
  138.     }

  139.     /** {@inheritDoc} */
  140.     @Override
  141.     public double getLatitudeStep() {
  142.         return latitudeStep;
  143.     }

  144.     /** {@inheritDoc} */
  145.     @Override
  146.     public double getLongitudeStep() {
  147.         return longitudeStep;
  148.     }

  149.     /** {@inheritDoc} */
  150.     @Override
  151.     public int getLatitudeRows() {
  152.         return latitudeRows;
  153.     }

  154.     /** {@inheritDoc} */
  155.     @Override
  156.     public int getLongitudeColumns() {
  157.         return longitudeColumns;
  158.     }

  159.     /** {@inheritDoc} */
  160.     @Override
  161.     public double getMinElevation() {
  162.         return minElevation;
  163.     }

  164.     /** {@inheritDoc} */
  165.     @Override
  166.     public int getMinElevationLatitudeIndex() {
  167.         return minElevationLatitudeIndex;
  168.     }

  169.     /** {@inheritDoc} */
  170.     @Override
  171.     public int getMinElevationLongitudeIndex() {
  172.         return minElevationLongitudeIndex;
  173.     }

  174.     /** {@inheritDoc} */
  175.     @Override
  176.     public double getMaxElevation() {
  177.         return maxElevation;
  178.     }

  179.     /** {@inheritDoc} */
  180.     @Override
  181.     public int getMaxElevationLatitudeIndex() {
  182.         return maxElevationLatitudeIndex;
  183.     }

  184.     /** {@inheritDoc} */
  185.     @Override
  186.     public int getMaxElevationLongitudeIndex() {
  187.         return maxElevationLongitudeIndex;
  188.     }

  189.     /** {@inheritDoc} */
  190.     @Override
  191.     public void setElevation(final int latitudeIndex, final int longitudeIndex, final double elevation) {

  192.         if (latitudeIndex  < 0 || latitudeIndex  > (latitudeRows - 1) ||
  193.             longitudeIndex < 0 || longitudeIndex > (longitudeColumns - 1)) {
  194.             throw new RuggedException(RuggedMessages.OUT_OF_TILE_INDICES,
  195.                                       latitudeIndex, longitudeIndex,
  196.                                       latitudeRows - 1, longitudeColumns - 1);
  197.         }
  198.         if (MinSelector.getInstance().selectFirst(elevation, minElevation)) {
  199.             minElevation               = elevation;
  200.             minElevationLatitudeIndex  = latitudeIndex;
  201.             minElevationLongitudeIndex = longitudeIndex;
  202.         }
  203.         if (MaxSelector.getInstance().selectFirst(elevation, maxElevation)) {
  204.             maxElevation               = elevation;
  205.             maxElevationLatitudeIndex  = latitudeIndex;
  206.             maxElevationLongitudeIndex = longitudeIndex;
  207.         }
  208.         elevations[latitudeIndex * getLongitudeColumns() + longitudeIndex] = elevation;
  209.     }

  210.     /** {@inheritDoc} */
  211.     @Override
  212.     public double getElevationAtIndices(final int latitudeIndex, final int longitudeIndex) {
  213.         final double elevation = elevations[latitudeIndex * getLongitudeColumns() + longitudeIndex];
  214.         DumpManager.dumpTileCell(this, latitudeIndex, longitudeIndex, elevation);
  215.         return elevation;
  216.     }

  217.     /** {@inheritDoc}
  218.      * <p>
  219.      * This classes uses an arbitrary 1/8 cell tolerance for interpolating
  220.      * slightly out of tile points.
  221.      * </p>
  222.      */
  223.     @Override
  224.     public double interpolateElevation(final double latitude, final double longitude) {

  225.         final double doubleLatitudeIndex  = getDoubleLatitudeIndex(latitude);
  226.         final double doubleLongitudeIndex = getDoubleLontitudeIndex(longitude);
  227.         if (doubleLatitudeIndex  < -TOLERANCE || doubleLatitudeIndex  >= (latitudeRows - 1 + TOLERANCE) ||
  228.             doubleLongitudeIndex < -TOLERANCE || doubleLongitudeIndex >= (longitudeColumns - 1 + TOLERANCE)) {
  229.             throw new RuggedException(RuggedMessages.OUT_OF_TILE_ANGLES,
  230.                                       FastMath.toDegrees(latitude),
  231.                                       FastMath.toDegrees(longitude),
  232.                                       FastMath.toDegrees(getMinimumLatitude()),
  233.                                       FastMath.toDegrees(getMaximumLatitude()),
  234.                                       FastMath.toDegrees(getMinimumLongitude()),
  235.                                       FastMath.toDegrees(getMaximumLongitude()));
  236.         }

  237.         final int latitudeIndex  = FastMath.max(0,
  238.                                                 FastMath.min(latitudeRows - 2,
  239.                                                              (int) FastMath.floor(doubleLatitudeIndex)));
  240.         final int longitudeIndex = FastMath.max(0,
  241.                                                 FastMath.min(longitudeColumns - 2,
  242.                                                              (int) FastMath.floor(doubleLongitudeIndex)));

  243.         // bi-linear interpolation
  244.         final double dLat = doubleLatitudeIndex  - latitudeIndex;
  245.         final double dLon = doubleLongitudeIndex - longitudeIndex;
  246.         final double e00  = getElevationAtIndices(latitudeIndex,     longitudeIndex);
  247.         final double e10  = getElevationAtIndices(latitudeIndex,     longitudeIndex + 1);
  248.         final double e01  = getElevationAtIndices(latitudeIndex + 1, longitudeIndex);
  249.         final double e11  = getElevationAtIndices(latitudeIndex + 1, longitudeIndex + 1);

  250.         return (e00 * (1.0 - dLon) + dLon * e10) * (1.0 - dLat) +
  251.                (e01 * (1.0 - dLon) + dLon * e11) * dLat;

  252.     }

  253.     /** {@inheritDoc} */
  254.     @Override
  255.     public NormalizedGeodeticPoint cellIntersection(final GeodeticPoint p, final Vector3D los,
  256.                                                     final int latitudeIndex, final int longitudeIndex) {

  257.         // ensure neighboring cells to not fall out of tile
  258.         final int iLat  = FastMath.max(0, FastMath.min(latitudeRows     - 2, latitudeIndex));
  259.         final int jLong = FastMath.max(0, FastMath.min(longitudeColumns - 2, longitudeIndex));

  260.         // Digital Elevation Mode coordinates at cell vertices
  261.         final double x00 = getLongitudeAtIndex(jLong);
  262.         final double y00 = getLatitudeAtIndex(iLat);
  263.         final double z00 = getElevationAtIndices(iLat,     jLong);
  264.         final double z01 = getElevationAtIndices(iLat + 1, jLong);
  265.         final double z10 = getElevationAtIndices(iLat,     jLong + 1);
  266.         final double z11 = getElevationAtIndices(iLat + 1, jLong + 1);

  267.         // line-of-sight coordinates at close points
  268.         final double dxA = (p.getLongitude() - x00) / longitudeStep;
  269.         final double dyA = (p.getLatitude()  - y00) / latitudeStep;
  270.         final double dzA = p.getAltitude();
  271.         final double dxB = dxA + los.getX() / longitudeStep;
  272.         final double dyB = dyA + los.getY() / latitudeStep;
  273.         final double dzB = dzA + los.getZ();

  274.         // points along line-of-sight can be defined as a linear progression
  275.         // along the line depending on free variable t: p(t) = p + t * los.
  276.         // As the point latitude and longitude are linear with respect to t,
  277.         // and as Digital Elevation Model is quadratic with respect to latitude
  278.         // and longitude, the altitude of DEM at same horizontal position as
  279.         // point is quadratic in t:
  280.         // z_DEM(t) = u t² + v t + w
  281.         final double u = (dxA - dxB) * (dyA - dyB) * (z00 - z10 - z01 + z11);
  282.         final double v = ((dxA - dxB) * (1 - dyA) + (dyA - dyB) * (1 - dxA)) * z00 +
  283.                          (dxA * (dyA - dyB) - (dxA - dxB) * (1 - dyA)) * z10 +
  284.                          (dyA * (dxA - dxB) - (dyA - dyB) * (1 - dxA)) * z01 +
  285.                          ((dxB - dxA) * dyA + (dyB - dyA) * dxA) * z11;
  286.         final double w = (1 - dxA) * ((1 - dyA) * z00 + dyA * z01) +
  287.                          dxA       * ((1 - dyA) * z10 + dyA * z11);

  288.         // subtract linear z from line-of-sight
  289.         // z_DEM(t) - z_LOS(t) = a t² + b t + c
  290.         final double a = u;
  291.         final double b = v + dzA - dzB;
  292.         final double c = w - dzA;

  293.         // solve the equation
  294.         final double t1;
  295.         final double t2;
  296.         if (FastMath.abs(a) <= Precision.EPSILON * FastMath.abs(c)) {
  297.             // the equation degenerates to a linear (or constant) equation
  298.             final double t = -c / b;
  299.             t1 = Double.isNaN(t) ? 0.0 : t;
  300.             t2 = Double.POSITIVE_INFINITY;
  301.         } else {
  302.             // the equation is quadratic
  303.             final double b2  = b * b;
  304.             final double fac = 4 * a * c;
  305.             if (b2 < fac) {
  306.                 // no intersection at all
  307.                 return null;
  308.             }
  309.             final double s = FastMath.sqrt(b2 - fac);
  310.             t1 = (b < 0) ? (s - b) / (2 * a) : -2 * c / (b + s);
  311.             t2 = c / (a * t1);

  312.         }

  313.         final NormalizedGeodeticPoint p1 = interpolate(t1, p, dxA, dyA, los, x00);
  314.         final NormalizedGeodeticPoint p2 = interpolate(t2, p, dxA, dyA, los, x00);

  315.         // select the first point along line-of-sight
  316.         if (p1 == null) {
  317.             return p2;
  318.         } else if (p2 == null) {
  319.             return p1;
  320.         } else {
  321.             return t1 <= t2 ? p1 : p2;
  322.         }

  323.     }

  324.     /** Interpolate point along a line.
  325.      * @param t abscissa along the line
  326.      * @param p start point
  327.      * @param dxP relative coordinate of the start point with respect to current cell
  328.      * @param dyP relative coordinate of the start point with respect to current cell
  329.      * @param los direction of the line-of-sight, in geodetic space
  330.      * @param centralLongitude reference longitude lc such that the point longitude will
  331.      * be normalized between lc-π and lc+π
  332.      * @return interpolated point along the line
  333.      */
  334.     private NormalizedGeodeticPoint interpolate(final double t, final GeodeticPoint p,
  335.                                                 final double dxP, final double dyP,
  336.                                                 final Vector3D los, final double centralLongitude) {

  337.         if (Double.isInfinite(t)) {
  338.             return null;
  339.         }

  340.         final double dx = dxP + t * los.getX() / longitudeStep;
  341.         final double dy = dyP + t * los.getY() / latitudeStep;
  342.         if (dx >= -TOLERANCE && dx <= 1 + TOLERANCE && dy >= -TOLERANCE && dy <= 1 + TOLERANCE) {
  343.             return new NormalizedGeodeticPoint(p.getLatitude()  + t * los.getY(),
  344.                                                p.getLongitude() + t * los.getX(),
  345.                                                p.getAltitude()  + t * los.getZ(),
  346.                                                centralLongitude);
  347.         } else {
  348.             return null;
  349.         }

  350.     }

  351.     /** {@inheritDoc} */
  352.     @Override
  353.     public int getFloorLatitudeIndex(final double latitude) {
  354.         return (int) FastMath.floor(getDoubleLatitudeIndex(latitude));
  355.     }

  356.     /** {@inheritDoc} */
  357.     @Override
  358.     public int getFloorLongitudeIndex(final double longitude) {
  359.         return (int) FastMath.floor(getDoubleLontitudeIndex(longitude));
  360.     }

  361.     /** Get the latitude index of a point.
  362.      * @param latitude geodetic latitude (rad)
  363.      * @return latitude index (it may lie outside of the tile!)
  364.      */
  365.     private double getDoubleLatitudeIndex(final double latitude) {
  366.         return (latitude  - minLatitude)  / latitudeStep;
  367.     }

  368.     /** Get the longitude index of a point.
  369.      * @param longitude geodetic longitude (rad)
  370.      * @return longitude index (it may lie outside of the tile!)
  371.      */
  372.     private double getDoubleLontitudeIndex(final double longitude) {
  373.         return (longitude - minLongitude) / longitudeStep;
  374.     }

  375.     /** {@inheritDoc} */
  376.     @Override
  377.     public Location getLocation(final double latitude, final double longitude) {
  378.         final int latitudeIndex  = getFloorLatitudeIndex(latitude);
  379.         final int longitudeIndex = getFloorLongitudeIndex(longitude);
  380.         if (longitudeIndex < 0) {
  381.             if (latitudeIndex < 0) {
  382.                 return Location.SOUTH_WEST;
  383.             } else if (latitudeIndex <= (latitudeRows - 2)) {
  384.                 return Location.WEST;
  385.             } else {
  386.                 return Location.NORTH_WEST;
  387.             }
  388.         } else if (longitudeIndex <= (longitudeColumns - 2)) {
  389.             if (latitudeIndex < 0) {
  390.                 return Location.SOUTH;
  391.             } else if (latitudeIndex <= (latitudeRows - 2)) {
  392.                 return Location.HAS_INTERPOLATION_NEIGHBORS;
  393.             } else {
  394.                 return Location.NORTH;
  395.             }
  396.         } else {
  397.             if (latitudeIndex < 0) {
  398.                 return Location.SOUTH_EAST;
  399.             } else if (latitudeIndex <= (latitudeRows - 2)) {
  400.                 return Location.EAST;
  401.             } else {
  402.                 return Location.NORTH_EAST;
  403.             }
  404.         }
  405.     }

  406. }