Rugged.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.api;

  19. import java.util.Collection;
  20. import java.util.HashMap;
  21. import java.util.Map;

  23. import org.hipparchus.analysis.differentiation.Derivative;
  24. import org.hipparchus.analysis.differentiation.DerivativeStructure;
  25. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  26. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  27. import org.hipparchus.util.FastMath;
  28. import org.hipparchus.util.MathArrays;
  29. import org.orekit.bodies.GeodeticPoint;
  30. import org.orekit.frames.Transform;
  31. import org.orekit.rugged.errors.DumpManager;
  32. import org.orekit.rugged.errors.RuggedException;
  33. import org.orekit.rugged.errors.RuggedInternalError;
  34. import org.orekit.rugged.errors.RuggedMessages;
  35. import org.orekit.rugged.intersection.IntersectionAlgorithm;
  36. import org.orekit.rugged.linesensor.LineSensor;
  37. import org.orekit.rugged.linesensor.SensorMeanPlaneCrossing;
  38. import org.orekit.rugged.linesensor.SensorPixel;
  39. import org.orekit.rugged.linesensor.SensorPixelCrossing;
  40. import org.orekit.rugged.refraction.AtmosphericRefraction;
  41. import org.orekit.rugged.utils.DerivativeGenerator;
  42. import org.orekit.rugged.utils.ExtendedEllipsoid;
  43. import org.orekit.rugged.utils.NormalizedGeodeticPoint;
  44. import org.orekit.rugged.utils.SpacecraftToObservedBody;
  45. import org.orekit.time.AbsoluteDate;
  46. import org.orekit.utils.Constants;
  47. import org.orekit.utils.PVCoordinates;

  49. /** Main class of Rugged library API.
  50.  * @see RuggedBuilder
  51.  * @author Luc Maisonobe
  52.  * @author Guylaine Prat
  53.  * @author Jonathan Guinet
  54.  * @author Lucie LabatAllee
  55.  */
  56. public class Rugged {

  58.     /** Accuracy to use in the first stage of inverse location.
  59.      * <p>
  60.      * This accuracy is only used to locate the point within one
  61.      * pixel, hence there is no point in choosing a too small value here.
  62.      * </p>
  63.      */
  64.     private static final double COARSE_INVERSE_LOCATION_ACCURACY = 0.01;

  66.     /** Maximum number of evaluations for crossing algorithms. */
  67.     private static final int MAX_EVAL = 50;

  69.     /** Threshold for pixel convergence in fixed point method
  70.      * (for inverse location with atmospheric refraction correction). */
  71.     private static final double PIXEL_CV_THRESHOLD = 1.e-4;

  73.     /** Threshold for line convergence in fixed point method
  74.      * (for inverse location with atmospheric refraction correction). */
  75.     private static final double LINE_CV_THRESHOLD = 1.e-4;

  77.     /** Reference ellipsoid. */
  78.     private final ExtendedEllipsoid ellipsoid;

  80.     /** Converter between spacecraft and body. */
  81.     private final SpacecraftToObservedBody scToBody;

  83.     /** Sensors. */
  84.     private final Map<String, LineSensor> sensors;

  86.     /** Mean plane crossing finders. */
  87.     private final Map<String, SensorMeanPlaneCrossing> finders;

  89.     /** DEM intersection algorithm. */
  90.     private final IntersectionAlgorithm algorithm;

  92.     /** Flag for light time correction. */
  93.     private boolean lightTimeCorrection;

  95.     /** Flag for aberration of light correction. */
  96.     private boolean aberrationOfLightCorrection;

  98.     /** Rugged name. */
  99.     private final String name;

  101.     /** Atmospheric refraction for line of sight correction. */
  102.     private AtmosphericRefraction atmosphericRefraction;

  104.     /** Build a configured instance.
  105.      * <p>
  106.      * By default, the instance performs both light time correction (which refers
  107.      * to ground point motion with respect to inertial frame) and aberration of
  108.      * light correction (which refers to spacecraft proper velocity). Explicit calls
  109.      * to {@link #setLightTimeCorrection(boolean) setLightTimeCorrection}
  110.      * and {@link #setAberrationOfLightCorrection(boolean) setAberrationOfLightCorrection}
  111.      * can be made after construction if these phenomena should not be corrected.
  112.      * </p>
  113.      * @param algorithm algorithm to use for Digital Elevation Model intersection
  114.      * @param ellipsoid f reference ellipsoid
  115.      * @param lightTimeCorrection if true, the light travel time between ground
  116.      * @param aberrationOfLightCorrection if true, the aberration of light
  117.      * is corrected for more accurate location
  118.      * and spacecraft is compensated for more accurate location
  119.      * @param atmosphericRefraction the atmospheric refraction model to be used for more accurate location
  120.      * @param scToBody transforms interpolator
  121.      * @param sensors sensors
  122.      * @param name Rugged name
  123.      */
  124.     Rugged(final IntersectionAlgorithm algorithm, final ExtendedEllipsoid ellipsoid, final boolean lightTimeCorrection,
  125.            final boolean aberrationOfLightCorrection, final AtmosphericRefraction atmosphericRefraction,
  126.            final SpacecraftToObservedBody scToBody, final Collection<LineSensor> sensors, final String name) {


  129.         // space reference
  130.         this.ellipsoid = ellipsoid;

  132.         // orbit/attitude to body converter
  133.         this.scToBody = scToBody;

  135.         // intersection algorithm
  136.         this.algorithm = algorithm;

  138.         // Rugged name
  139.         // @since 2.0
  140.         this.name = name;

  142.         this.sensors = new HashMap<String, LineSensor>();
  143.         for (final LineSensor s : sensors) {
  144.             this.sensors.put(s.getName(), s);
  145.         }
  146.         this.finders = new HashMap<String, SensorMeanPlaneCrossing>();

  148.         this.lightTimeCorrection         = lightTimeCorrection;
  149.         this.aberrationOfLightCorrection = aberrationOfLightCorrection;
  150.         this.atmosphericRefraction       = atmosphericRefraction;
  151.     }

  153.     /** Get the Rugged name.
  154.      * @return Rugged name
  155.      * @since 2.0
  156.      */
  157.     public String getName() {
  158.         return name;
  159.     }

  161.     /** Get the DEM intersection algorithm.
  162.      * @return DEM intersection algorithm
  163.      */
  164.     public IntersectionAlgorithm getAlgorithm() {
  165.         return algorithm;
  166.     }

  168.     /** Get the DEM intersection algorithm identifier.
  169.      * @return DEM intersection algorithm Id
  170.      * @since 2.2
  171.      */
  172.     public AlgorithmId getAlgorithmId() {
  173.         return algorithm.getAlgorithmId();
  174.     }

  176.     /** Get flag for light time correction.
  177.      * @return true if the light time between ground and spacecraft is
  178.      * compensated for more accurate location
  179.      */
  180.     public boolean isLightTimeCorrected() {
  181.         return lightTimeCorrection;
  182.     }

  184.     /** Get flag for aberration of light correction.
  185.      * @return true if the aberration of light time is corrected
  186.      * for more accurate location
  187.      */
  188.     public boolean isAberrationOfLightCorrected() {
  189.         return aberrationOfLightCorrection;
  190.     }

  192.     /** Get the atmospheric refraction model.
  193.      * @return atmospheric refraction model
  194.      * @since 2.0
  195.      */
  196.     public AtmosphericRefraction getRefractionCorrection() {
  197.         return atmosphericRefraction;
  198.     }

  200.     /** Get the line sensors.
  201.      * @return line sensors
  202.      */
  203.     public Collection<LineSensor> getLineSensors() {
  204.         return sensors.values();
  205.     }

  207.     /** Get the start of search time span.
  208.      * @return start of search time span
  209.      */
  210.     public AbsoluteDate getMinDate() {
  211.         return scToBody.getMinDate();
  212.     }

  214.     /** Get the end of search time span.
  215.      * @return end of search time span
  216.      */
  217.     public AbsoluteDate getMaxDate() {
  218.         return scToBody.getMaxDate();
  219.     }

  221.     /** Check if a date is in the supported range.
  222.      * <p>
  223.      * The support range is given by the {@code minDate} and
  224.      * {@code maxDate} construction parameters, with an {@code
  225.      * overshootTolerance} margin accepted (i.e. a date slightly
  226.      * before {@code minDate} or slightly after {@code maxDate}
  227.      * will be considered in range if the overshoot does not exceed
  228.      * the tolerance set at construction).
  229.      * </p>
  230.      * @param date date to check
  231.      * @return true if date is in the supported range
  232.      */
  233.     public boolean isInRange(final AbsoluteDate date) {
  234.         return scToBody.isInRange(date);
  235.     }

  237.     /** Get the observed body ellipsoid.
  238.      * @return observed body ellipsoid
  239.      */
  240.     public ExtendedEllipsoid getEllipsoid() {
  241.         return ellipsoid;
  242.     }

  244.     /** Direct location of a sensor line.
  245.      * @param sensorName name of the line sensor
  246.      * @param lineNumber number of the line to localize on ground
  247.      * @return ground position of all pixels of the specified sensor line
  248.      */
  249.     public GeodeticPoint[] directLocation(final String sensorName, final double lineNumber) {

  251.         final LineSensor   sensor = getLineSensor(sensorName);
  252.         final Vector3D sensorPosition   = sensor.getPosition();
  253.         final AbsoluteDate date   = sensor.getDate(lineNumber);

  255.         // Compute the transform for the date
  256.         // from spacecraft to inertial
  257.         final Transform    scToInert   = scToBody.getScToInertial(date);
  258.         // from inertial to body
  259.         final Transform    inertToBody = scToBody.getInertialToBody(date);

  261.         // Compute spacecraft velocity in inertial frame
  262.         final Vector3D spacecraftVelocity = scToInert.transformPVCoordinates(PVCoordinates.ZERO).getVelocity();
  263.         // Compute sensor position in inertial frame
  264.         // TBN: for simplicity, due to the size of sensor, we consider each pixel to be at sensor position
  265.         final Vector3D pInert = scToInert.transformPosition(sensorPosition);

  267.         // Compute location of each pixel
  268.         final GeodeticPoint[] gp = new GeodeticPoint[sensor.getNbPixels()];
  269.         for (int i = 0; i < sensor.getNbPixels(); ++i) {

  271.             final Vector3D los = sensor.getLOS(date, i);
  272.             DumpManager.dumpDirectLocation(date, sensorPosition, los, lightTimeCorrection,
  273.                     aberrationOfLightCorrection, atmosphericRefraction != null);

  275.             // compute the line of sight in inertial frame (without correction)
  276.             final Vector3D obsLInert = scToInert.transformVector(los);
  277.             final Vector3D lInert;

  279.             if (aberrationOfLightCorrection) {
  280.                 // apply aberration of light correction on LOS
  281.                 lInert = applyAberrationOfLightCorrection(obsLInert, spacecraftVelocity);
  282.             } else {
  283.                 // don't apply aberration of light correction on LOS
  284.                 lInert = obsLInert;
  285.             }

  287.             if (lightTimeCorrection) {
  288.                 // compute DEM intersection with light time correction
  289.                 // TBN: for simplicity, due to the size of sensor, we consider each pixel to be at sensor position
  290.                 gp[i] = computeWithLightTimeCorrection(date, sensorPosition, los, scToInert, inertToBody, pInert, lInert);

  292.             } else {
  293.                 // compute DEM intersection without light time correction
  294.                 final Vector3D pBody = inertToBody.transformPosition(pInert);
  295.                 final Vector3D lBody = inertToBody.transformVector(lInert);
  296.                 gp[i] = algorithm.refineIntersection(ellipsoid, pBody, lBody,
  297.                                                      algorithm.intersection(ellipsoid, pBody, lBody));
  298.             }

  300.             // compute with atmospheric refraction correction if necessary
  301.             if (atmosphericRefraction != null && atmosphericRefraction.mustBeComputed()) {

  303.                 // apply atmospheric refraction correction
  304.                 final Vector3D pBody = inertToBody.transformPosition(pInert);
  305.                 final Vector3D lBody = inertToBody.transformVector(lInert);
  306.                 gp[i] = atmosphericRefraction.applyCorrection(pBody, lBody, (NormalizedGeodeticPoint) gp[i], algorithm);
  307.             }
  308.             DumpManager.dumpDirectLocationResult(gp[i]);
  309.         }
  310.         return gp;
  311.     }

  313.     /** Direct location of a single line-of-sight.
  314.      * @param date date of the location
  315.      * @param sensorPosition sensor position in spacecraft frame. For simplicity, due to the size of sensor,
  316.      * we consider each pixel to be at sensor position
  317.      * @param los normalized line-of-sight in spacecraft frame
  318.      * @return ground position of intersection point between specified los and ground
  319.      */
  320.     public GeodeticPoint directLocation(final AbsoluteDate date, final Vector3D sensorPosition, final Vector3D los) {

  322.         DumpManager.dumpDirectLocation(date, sensorPosition, los, lightTimeCorrection, aberrationOfLightCorrection,
  323.                                        atmosphericRefraction != null);

  325.         // Compute the transforms for the date
  326.         // from spacecraft to inertial
  327.         final Transform    scToInert   = scToBody.getScToInertial(date);
  328.         // from inertial to body
  329.         final Transform    inertToBody = scToBody.getInertialToBody(date);

  331.         // Compute spacecraft velocity in inertial frame
  332.         final Vector3D spacecraftVelocity = scToInert.transformPVCoordinates(PVCoordinates.ZERO).getVelocity();
  333.         // Compute sensor position in inertial frame
  334.         // TBN: for simplicity, due to the size of sensor, we consider each pixel to be at sensor position
  335.         final Vector3D pInert    = scToInert.transformPosition(sensorPosition);

  337.         // Compute the line of sight in inertial frame (without correction)
  338.         final Vector3D obsLInert = scToInert.transformVector(los);

  340.         final Vector3D lInert;
  341.         if (aberrationOfLightCorrection) {
  342.             // apply aberration of light correction on LOS
  343.             lInert = applyAberrationOfLightCorrection(obsLInert, spacecraftVelocity);
  344.         } else {
  345.             // don't apply aberration of light correction on LOS
  346.             lInert = obsLInert;
  347.         }

  349.         // Compute ground location of specified pixel
  350.         final NormalizedGeodeticPoint gp;

  352.         if (lightTimeCorrection) {
  353.             // compute DEM intersection with light time correction
  354.             // TBN: for simplicity, due to the size of sensor, we consider each pixel to be at sensor position
  355.             gp = computeWithLightTimeCorrection(date, sensorPosition, los, scToInert, inertToBody, pInert, lInert);

  357.         } else {
  358.             // compute DEM intersection without light time correction
  359.             final Vector3D pBody = inertToBody.transformPosition(pInert);
  360.             final Vector3D lBody = inertToBody.transformVector(lInert);
  361.             gp = algorithm.refineIntersection(ellipsoid, pBody, lBody,
  362.                                               algorithm.intersection(ellipsoid, pBody, lBody));
  363.         }

  365.         NormalizedGeodeticPoint result = gp;

  367.         // compute the ground location with atmospheric correction if asked for
  368.         if (atmosphericRefraction != null && atmosphericRefraction.mustBeComputed()) {

  370.             // apply atmospheric refraction correction
  371.             final Vector3D pBody = inertToBody.transformPosition(pInert);
  372.             final Vector3D lBody = inertToBody.transformVector(lInert);
  373.             result = atmosphericRefraction.applyCorrection(pBody, lBody, gp, algorithm);

  375.         } // end test on atmosphericRefraction != null

  377.         DumpManager.dumpDirectLocationResult(result);
  378.         return result;
  379.     }

  381.     /** Find the date at which sensor sees a ground point.
  382.      * <p>
  383.      * This method is a partial {@link #inverseLocation(String, GeodeticPoint, int, int) inverse location} focusing only on date.
  384.      * </p>
  385.      * <p>
  386.      * The point is given only by its latitude and longitude, the elevation is
  387.      * computed from the Digital Elevation Model.
  388.      * </p>
  389.      * <p>
  390.      * Note that for each sensor name, the {@code minLine} and {@code maxLine} settings
  391.      * are cached, because they induce costly frames computation. So these settings
  392.      * should not be tuned very finely and changed at each call, but should rather be
  393.      * a few thousand lines wide and refreshed only when needed. If for example an
  394.      * inverse location is roughly estimated to occur near line 53764 (for example
  395.      * using {@link org.orekit.rugged.utils.RoughVisibilityEstimator}), {@code minLine}
  396.      * and {@code maxLine} could be set for example to 50000 and 60000, which would
  397.      * be OK also if next line inverse location is expected to occur near line 53780,
  398.      * and next one ... The setting could be changed for example to 55000 and 65000 when
  399.      * an inverse location is expected to occur after 55750. Of course, these values
  400.      * are only an example and should be adjusted depending on mission needs.
  401.      * </p>
  402.      * @param sensorName name of the line  sensor
  403.      * @param latitude ground point latitude (rad)
  404.      * @param longitude ground point longitude (rad)
  405.      * @param minLine minimum line number
  406.      * @param maxLine maximum line number
  407.      * @return date at which ground point is seen by line sensor
  408.      * @see #inverseLocation(String, double, double, int, int)
  409.      * @see org.orekit.rugged.utils.RoughVisibilityEstimator
  410.      */
  411.     public AbsoluteDate dateLocation(final String sensorName,
  412.                                      final double latitude, final double longitude,
  413.                                      final int minLine, final int maxLine) {

  415.         final GeodeticPoint groundPoint =
  416.                 new GeodeticPoint(latitude, longitude, algorithm.getElevation(latitude, longitude));
  417.         return dateLocation(sensorName, groundPoint, minLine, maxLine);
  418.     }

  420.     /** Find the date at which sensor sees a ground point.
  421.      * <p>
  422.      * This method is a partial {@link #inverseLocation(String,
  423.      * GeodeticPoint, int, int) inverse location} focusing only on date.
  424.      * </p>
  425.      * <p>
  426.      * Note that for each sensor name, the {@code minLine} and {@code maxLine} settings
  427.      * are cached, because they induce costly frames computation. So these settings
  428.      * should not be tuned very finely and changed at each call, but should rather be
  429.      * a few thousand lines wide and refreshed only when needed. If for example an
  430.      * inverse location is roughly estimated to occur near line 53764 (for example
  431.      * using {@link org.orekit.rugged.utils.RoughVisibilityEstimator}), {@code minLine}
  432.      * and {@code maxLine} could be set for example to 50000 and 60000, which would
  433.      * be OK also if next line inverse location is expected to occur near line 53780,
  434.      * and next one ... The setting could be changed for example to 55000 and 65000 when
  435.      * an inverse location is expected to occur after 55750. Of course, these values
  436.      * are only an example and should be adjusted depending on mission needs.
  437.      * </p>
  438.      * @param sensorName name of the line sensor
  439.      * @param point point to localize
  440.      * @param minLine minimum line number
  441.      * @param maxLine maximum line number
  442.      * @return date at which ground point is seen by line sensor
  443.      * @see #inverseLocation(String, GeodeticPoint, int, int)
  444.      * @see org.orekit.rugged.utils.RoughVisibilityEstimator
  445.      */
  446.     public AbsoluteDate dateLocation(final String sensorName, final GeodeticPoint point,
  447.                                      final int minLine, final int maxLine) {

  449.         final LineSensor sensor = getLineSensor(sensorName);
  450.         final SensorMeanPlaneCrossing planeCrossing = getPlaneCrossing(sensorName, minLine, maxLine);

  452.         // find approximately the sensor line at which ground point crosses sensor mean plane
  453.         final Vector3D   target = ellipsoid.transform(point);
  454.         final SensorMeanPlaneCrossing.CrossingResult crossingResult = planeCrossing.find(target);
  455.         if (crossingResult == null) {
  456.             // target is out of search interval
  457.             return null;
  458.         } else {
  459.             return sensor.getDate(crossingResult.getLine());
  460.         }
  461.     }

  463.     /** Inverse location of a ground point.
  464.      * <p>
  465.      * The point is given only by its latitude and longitude, the elevation is
  466.      * computed from the Digital Elevation Model.
  467.      * </p>
  468.      * <p>
  469.      * Note that for each sensor name, the {@code minLine} and {@code maxLine} settings
  470.      * are cached, because they induce costly frames computation. So these settings
  471.      * should not be tuned very finely and changed at each call, but should rather be
  472.      * a few thousand lines wide and refreshed only when needed. If for example an
  473.      * inverse location is roughly estimated to occur near line 53764 (for example
  474.      * using {@link org.orekit.rugged.utils.RoughVisibilityEstimator}), {@code minLine}
  475.      * and {@code maxLine} could be set for example to 50000 and 60000, which would
  476.      * be OK also if next line inverse location is expected to occur near line 53780,
  477.      * and next one ... The setting could be changed for example to 55000 and 65000 when
  478.      * an inverse location is expected to occur after 55750. Of course, these values
  479.      * are only an example and should be adjusted depending on mission needs.
  480.      * </p>
  481.      * @param sensorName name of the line  sensor
  482.      * @param latitude ground point latitude (rad)
  483.      * @param longitude ground point longitude (rad)
  484.      * @param minLine minimum line number
  485.      * @param maxLine maximum line number
  486.      * @return sensor pixel seeing ground point, or null if ground point cannot
  487.      * be seen between the prescribed line numbers
  488.      * @see org.orekit.rugged.utils.RoughVisibilityEstimator
  489.      */
  490.     public SensorPixel inverseLocation(final String sensorName,
  491.                                        final double latitude, final double longitude,
  492.                                        final int minLine,  final int maxLine) {

  494.         final GeodeticPoint groundPoint = new GeodeticPoint(latitude, longitude, algorithm.getElevation(latitude, longitude));
  495.         return inverseLocation(sensorName, groundPoint, minLine, maxLine);
  496.     }

  498.     /** Inverse location of a point.
  499.      * <p>
  500.      * Note that for each sensor name, the {@code minLine} and {@code maxLine} settings
  501.      * are cached, because they induce costly frames computation. So these settings
  502.      * should not be tuned very finely and changed at each call, but should rather be
  503.      * a few thousand lines wide and refreshed only when needed. If for example an
  504.      * inverse location is roughly estimated to occur near line 53764 (for example
  505.      * using {@link org.orekit.rugged.utils.RoughVisibilityEstimator}), {@code minLine}
  506.      * and {@code maxLine} could be set for example to 50000 and 60000, which would
  507.      * be OK also if next line inverse location is expected to occur near line 53780,
  508.      * and next one ... The setting could be changed for example to 55000 and 65000 when
  509.      * an inverse location is expected to occur after 55750. Of course, these values
  510.      * are only an example and should be adjusted depending on mission needs.
  511.      * </p>
  512.      * @param sensorName name of the line sensor
  513.      * @param point geodetic point to localize
  514.      * @param minLine minimum line number where the search will be performed
  515.      * @param maxLine maximum line number where the search will be performed
  516.      * @return sensor pixel seeing point, or null if point cannot be seen between the
  517.      * prescribed line numbers
  518.      * @see #dateLocation(String, GeodeticPoint, int, int)
  519.      * @see org.orekit.rugged.utils.RoughVisibilityEstimator
  520.      */
  521.     public SensorPixel inverseLocation(final String sensorName, final GeodeticPoint point,
  522.                                        final int minLine, final int maxLine) {

  524.         final LineSensor sensor = getLineSensor(sensorName);
  525.         DumpManager.dumpInverseLocation(sensor, point, ellipsoid, minLine, maxLine, lightTimeCorrection,
  526.                                         aberrationOfLightCorrection, atmosphericRefraction != null);

  528.         final SensorMeanPlaneCrossing planeCrossing = getPlaneCrossing(sensorName, minLine, maxLine);
  529.         DumpManager.dumpSensorMeanPlane(planeCrossing);

  531.         if (atmosphericRefraction == null || !atmosphericRefraction.mustBeComputed()) {
  532.             // Compute inverse location WITHOUT atmospheric refraction
  533.             return findSensorPixelWithoutAtmosphere(point, sensor, planeCrossing);
  534.         } else {
  535.             // Compute inverse location WITH atmospheric refraction
  536.             return findSensorPixelWithAtmosphere(point, sensor, minLine, maxLine);
  537.         }
  538.     }

  540.     /** Apply aberration of light correction (for direct location).
  541.      * @param spacecraftVelocity spacecraft velocity in inertial frame
  542.      * @param obsLInert line of sight in inertial frame
  543.      * @return line of sight with aberration of light correction
  544.      */
  545.     private Vector3D applyAberrationOfLightCorrection(final Vector3D obsLInert, final Vector3D spacecraftVelocity) {

  547.         // As the spacecraft velocity is small with respect to speed of light,
  548.         // we use classical velocity addition and not relativistic velocity addition
  549.         // we look for a positive k such that: c * lInert + vsat = k * obsLInert
  550.         // with lInert normalized
  551.         final double a = obsLInert.getNormSq();
  552.         final double b = -Vector3D.dotProduct(obsLInert, spacecraftVelocity);
  553.         final double c = spacecraftVelocity.getNormSq() - Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT;

  555.         // a > 0 and c < 0
  556.         final double s = FastMath.sqrt(b * b - a * c);

  558.         // Only the k > 0 are kept as solutions (the solutions: -(s+b)/a and c/(s-b) are useless)
  559.         final double k = (b > 0) ? -c / (s + b) : (s - b) / a;

  561.         final Vector3D lInert = new Vector3D( k / Constants.SPEED_OF_LIGHT, obsLInert, -1.0 / Constants.SPEED_OF_LIGHT, spacecraftVelocity);
  562.         return lInert;
  563.     }

  565.     /** Compute the DEM intersection with light time correction.
  566.      * @param date date of the los
  567.      * @param sensorPosition sensor position in spacecraft frame
  568.      * @param los los in spacecraft frame
  569.      * @param scToInert transform for the date from spacecraft to inertial
  570.      * @param inertToBody transform for the date from inertial to body
  571.      * @param pInert sensor position in inertial frame
  572.      * @param lInert line of sight in inertial frame
  573.      * @return geodetic point with light time correction
  574.      */
  575.     private NormalizedGeodeticPoint computeWithLightTimeCorrection(final AbsoluteDate date,
  576.                                                                    final Vector3D sensorPosition, final Vector3D los,
  577.                                                                    final Transform scToInert, final Transform inertToBody,
  578.                                                                    final Vector3D pInert, final Vector3D lInert) {

  580.         // compute the approximate transform between spacecraft and observed body
  581.         final Transform approximate = new Transform(date, scToInert, inertToBody);

  583.         final Vector3D  sL       = approximate.transformVector(los);
  584.         final Vector3D  sP       = approximate.transformPosition(sensorPosition);

  586.         final Vector3D  eP1      = ellipsoid.transform(ellipsoid.pointOnGround(sP, sL, 0.0));
  587.         final double    deltaT1  = eP1.distance(sP) / Constants.SPEED_OF_LIGHT;
  588.         final Transform shifted1 = inertToBody.shiftedBy(-deltaT1);
  589.         final NormalizedGeodeticPoint gp1  = algorithm.intersection(ellipsoid,
  590.                                                                     shifted1.transformPosition(pInert),
  591.                                                                     shifted1.transformVector(lInert));

  593.         final Vector3D  eP2      = ellipsoid.transform(gp1);
  594.         final double    deltaT2  = eP2.distance(sP) / Constants.SPEED_OF_LIGHT;
  595.         final Transform shifted2 = inertToBody.shiftedBy(-deltaT2);
  596.         return algorithm.refineIntersection(ellipsoid,
  597.                                              shifted2.transformPosition(pInert),
  598.                                              shifted2.transformVector(lInert),
  599.                                              gp1);
  600.     }

  602.     /**
  603.      * Find the sensor pixel WITHOUT atmospheric refraction correction.
  604.      * @param point geodetic point to localize
  605.      * @param sensor the line sensor
  606.      * @param planeCrossing the sensor mean plane crossing
  607.      * @return the sensor pixel crossing or null if cannot be found
  608.      * @since 2.1
  609.      */
  610.     private SensorPixel findSensorPixelWithoutAtmosphere(final GeodeticPoint point,
  611.                                                          final LineSensor sensor, final SensorMeanPlaneCrossing planeCrossing) {

  613.         // find approximately the sensor line at which ground point crosses sensor mean plane
  614.         final Vector3D target = ellipsoid.transform(point);
  615.         final SensorMeanPlaneCrossing.CrossingResult crossingResult = planeCrossing.find(target);
  616.         if (crossingResult == null) {
  617.             // target is out of search interval
  618.             return null;
  619.         }

  621.         // find approximately the pixel along this sensor line
  622.         final SensorPixelCrossing pixelCrossing =
  623.                 new SensorPixelCrossing(sensor, planeCrossing.getMeanPlaneNormal(),
  624.                                         crossingResult.getTargetDirection(),
  625.                                         MAX_EVAL, COARSE_INVERSE_LOCATION_ACCURACY);
  626.         final double coarsePixel = pixelCrossing.locatePixel(crossingResult.getDate());
  627.         if (Double.isNaN(coarsePixel)) {
  628.             // target is out of search interval
  629.             return null;
  630.         }

  632.         // fix line by considering the closest pixel exact position and line-of-sight
  633.         // (this pixel might point towards a direction slightly above or below the mean sensor plane)
  634.         final int      lowIndex       = FastMath.max(0, FastMath.min(sensor.getNbPixels() - 2, (int) FastMath.floor(coarsePixel)));
  635.         final Vector3D lowLOS         = sensor.getLOS(crossingResult.getDate(), lowIndex);
  636.         final Vector3D highLOS        = sensor.getLOS(crossingResult.getDate(), lowIndex + 1);
  637.         final Vector3D localZ         = Vector3D.crossProduct(lowLOS, highLOS).normalize();
  638.         final double   beta           = FastMath.acos(Vector3D.dotProduct(crossingResult.getTargetDirection(), localZ));
  639.         final double   s              = Vector3D.dotProduct(crossingResult.getTargetDirectionDerivative(), localZ);
  640.         final double   betaDer        = -s / FastMath.sqrt(1 - s * s);
  641.         final double   deltaL         = (0.5 * FastMath.PI - beta) / betaDer;
  642.         final double   fixedLine      = crossingResult.getLine() + deltaL;
  643.         final Vector3D fixedDirection = new Vector3D(1, crossingResult.getTargetDirection(),
  644.                                                      deltaL, crossingResult.getTargetDirectionDerivative()).normalize();

  646.         // fix neighbouring pixels
  647.         final AbsoluteDate fixedDate  = sensor.getDate(fixedLine);
  648.         final Vector3D fixedX         = sensor.getLOS(fixedDate, lowIndex);
  649.         final Vector3D fixedZ         = Vector3D.crossProduct(fixedX, sensor.getLOS(fixedDate, lowIndex + 1));
  650.         final Vector3D fixedY         = Vector3D.crossProduct(fixedZ, fixedX);

  652.         // fix pixel
  653.         final double pixelWidth = FastMath.atan2(Vector3D.dotProduct(highLOS,        fixedY),
  654.                                                  Vector3D.dotProduct(highLOS,        fixedX));
  655.         final double alpha      = FastMath.atan2(Vector3D.dotProduct(fixedDirection, fixedY),
  656.                                                  Vector3D.dotProduct(fixedDirection, fixedX));
  657.         final double fixedPixel = lowIndex + alpha / pixelWidth;

  659.         final SensorPixel result = new SensorPixel(fixedLine, fixedPixel);
  660.         DumpManager.dumpInverseLocationResult(result);

  662.         return result;
  663.     }

  665.     /**
  666.      * Find the sensor pixel WITH atmospheric refraction correction.
  667.      * @param point geodetic point to localize
  668.      * @param sensor the line sensor
  669.      * @param minLine minimum line number where the search will be performed
  670.      * @param maxLine maximum line number where the search will be performed
  671.      * @return the sensor pixel crossing or null if cannot be found
  672.      * @since 2.1
  673.      */
  674.     private SensorPixel findSensorPixelWithAtmosphere(final GeodeticPoint point,
  675.                                                       final LineSensor sensor, final int minLine, final int maxLine) {

  677.         // TBN: there is no direct way to compute the inverse location.
  678.         // The method is based on an interpolation grid associated with the fixed point method

  680.         final String sensorName = sensor.getName();

  682.         // Compute a correction grid (at sensor level)
  683.         // ===========================================
  684.         // Need to be computed only once for a given sensor (with the same minLine and maxLine)
  685.         if (atmosphericRefraction.getBifPixel() == null || atmosphericRefraction.getBifLine() == null || // lazy evaluation
  686.             !atmosphericRefraction.isSameContext(sensorName, minLine, maxLine)) { // Must be recomputed if the context changed

  688.             // Definition of a regular grid (at sensor level)
  689.             atmosphericRefraction.configureCorrectionGrid(sensor, minLine, maxLine);

  691.             // Get the grid nodes
  692.             final int nbPixelGrid = atmosphericRefraction.getComputationParameters().getNbPixelGrid();
  693.             final int nbLineGrid = atmosphericRefraction.getComputationParameters().getNbLineGrid();
  694.             final double[] pixelGrid = atmosphericRefraction.getComputationParameters().getUgrid();
  695.             final double[] lineGrid = atmosphericRefraction.getComputationParameters().getVgrid();

  697.             // Computation, for the sensor grid, of the direct location WITH atmospheric refraction
  698.             // (full computation)
  699.             atmosphericRefraction.reactivateComputation();
  700.             final GeodeticPoint[][] geodeticGridWithAtmosphere = computeDirectLocOnGridWithAtmosphere(pixelGrid, lineGrid, sensor);
  701.             // pixelGrid and lineGrid are the nodes where the direct loc is computed WITH atmosphere

  703.             // Computation of the inverse location WITHOUT atmospheric refraction for the grid nodes
  704.             atmosphericRefraction.deactivateComputation();
  705.             final SensorPixel[][] sensorPixelGridInverseWithout = computeInverseLocOnGridWithoutAtmosphere(geodeticGridWithAtmosphere,
  706.                                                             nbPixelGrid, nbLineGrid, sensor, minLine, maxLine);
  707.             atmosphericRefraction.reactivateComputation();

  709.             // Compute the grid correction functions (for pixel and line)
  710.             atmosphericRefraction.computeGridCorrectionFunctions(sensorPixelGridInverseWithout);
  711.         }

  713.         // Fixed point method
  714.         // ==================
  715.         // Initialization
  716.         // --------------
  717.         // Deactivate the dump because no need to keep intermediate computations of inverse loc (can be regenerate)
  718.         final Boolean wasSuspended = DumpManager.suspend();

  720.         // compute the sensor pixel on the desired ground point WITHOUT atmosphere
  721.         atmosphericRefraction.deactivateComputation();
  722.         final SensorPixel sp0 = inverseLocation(sensorName, point, minLine, maxLine);
  723.         atmosphericRefraction.reactivateComputation();
  724.         // Reactivate the dump
  725.         DumpManager.resume(wasSuspended);

  727.         if (sp0 == null) {
  728.             // In order for the dump to end nicely
  729.             DumpManager.endNicely();

  731.             // Impossible to find the sensor pixel in the given range lines (without atmosphere)
  732.             throw new RuggedException(RuggedMessages.SENSOR_PIXEL_NOT_FOUND_IN_RANGE_LINES, minLine, maxLine);
  733.         }

  735.         // set up the starting point of the fixed point method
  736.         final double pixel0 = sp0.getPixelNumber();
  737.         final double line0 = sp0.getLineNumber();
  738.         // Needed data for the dump
  739.         sensor.dumpRate(line0);

  741.         // Apply fixed point method until convergence in pixel and line
  742.         // ------------------------------------------------------------
  743.         // compute the first (pixel, line) value:
  744.         // initial sensor pixel value + correction due to atmosphere at this same sensor pixel
  745.         double corrPixelPrevious =  pixel0 + atmosphericRefraction.getBifPixel().value(pixel0, line0);
  746.         double corrLinePrevious = line0 + atmosphericRefraction.getBifLine().value(pixel0, line0);

  748.         double deltaCorrPixel = Double.POSITIVE_INFINITY;
  749.         double deltaCorrLine = Double.POSITIVE_INFINITY;

  751.         while (deltaCorrPixel > PIXEL_CV_THRESHOLD && deltaCorrLine > LINE_CV_THRESHOLD) {
  752.             // Compute the current (pixel, line) value =
  753.             // initial sensor pixel value + correction due to atmosphere on the previous sensor pixel
  754.             final double corrPixelCurrent = pixel0 + atmosphericRefraction.getBifPixel().value(corrPixelPrevious, corrLinePrevious);
  755.             final double corrLineCurrent = line0 + atmosphericRefraction.getBifLine().value(corrPixelPrevious, corrLinePrevious);

  757.             // Compute the delta in pixel and line to check the convergence
  758.             deltaCorrPixel = FastMath.abs(corrPixelCurrent - corrPixelPrevious);
  759.             deltaCorrLine = FastMath.abs(corrLineCurrent - corrLinePrevious);

  761.             // Store the (pixel, line) for next loop
  762.             corrPixelPrevious = corrPixelCurrent;
  763.             corrLinePrevious = corrLineCurrent;
  764.         }
  765.         // The sensor pixel is found !
  766.         final SensorPixel sensorPixelWithAtmosphere = new SensorPixel(corrLinePrevious, corrPixelPrevious);

  768.         // Dump the found sensorPixel
  769.         DumpManager.dumpInverseLocationResult(sensorPixelWithAtmosphere);

  771.         return sensorPixelWithAtmosphere;
  772.     }

  774.     /** Compute the inverse location WITHOUT atmospheric refraction for the geodetic points
  775.      * associated to the sensor grid nodes.
  776.      * @param groundGridWithAtmosphere ground grid found for sensor grid nodes with atmosphere
  777.      * @param nbPixelGrid size of the pixel grid
  778.      * @param nbLineGrid size of the line grid
  779.      * @param sensor the line sensor
  780.      * @param minLine minimum line number where the search will be performed
  781.      * @param maxLine maximum line number where the search will be performed
  782.      * @return the sensor pixel grid computed without atmosphere
  783.      * @since 2.1
  784.      */
  785.     private SensorPixel[][] computeInverseLocOnGridWithoutAtmosphere(final GeodeticPoint[][] groundGridWithAtmosphere,
  786.                                                                      final int nbPixelGrid, final int nbLineGrid,
  787.                                                                      final LineSensor sensor, final int minLine, final int maxLine) {

  789.         // Deactivate the dump because no need to keep intermediate computations of inverse loc (can be regenerate)
  790.         final Boolean wasSuspended = DumpManager.suspend();

  792.         final SensorPixel[][] sensorPixelGrid = new SensorPixel[nbPixelGrid][nbLineGrid];
  793.         final String sensorName = sensor.getName();

  795.         for (int uIndex = 0; uIndex < nbPixelGrid; uIndex++) {
  796.             for (int vIndex = 0; vIndex < nbLineGrid; vIndex++) {

  798.                 // Check if the geodetic point exists
  799.                 if (groundGridWithAtmosphere[uIndex][vIndex] != null) {
  800.                     final GeodeticPoint groundPoint = groundGridWithAtmosphere[uIndex][vIndex];
  801.                     final double currentLat = groundPoint.getLatitude();
  802.                     final double currentLon = groundPoint.getLongitude();

  804.                     try {
  805.                         // Compute the inverse location for the current node
  806.                         sensorPixelGrid[uIndex][vIndex] = inverseLocation(sensorName, currentLat, currentLon, minLine, maxLine);

  808.                     } catch (RuggedException re) { // This should never happen
  809.                         // In order for the dump to end nicely
  810.                         DumpManager.endNicely();
  811.                         throw new RuggedInternalError(re);
  812.                     }

  814.                     // Check if the pixel is inside the sensor (with a margin) OR if the inverse location was impossible (null result)
  815.                     if (!pixelIsInside(sensorPixelGrid[uIndex][vIndex], sensor)) {

  817.                         // In order for the dump to end nicely
  818.                         DumpManager.endNicely();

  820.                         if (sensorPixelGrid[uIndex][vIndex] == null) {
  821.                             // Impossible to find the sensor pixel in the given range lines
  822.                             throw new RuggedException(RuggedMessages.SENSOR_PIXEL_NOT_FOUND_IN_RANGE_LINES, minLine, maxLine);
  823.                         } else {
  824.                             // Impossible to find the sensor pixel
  825.                             final double invLocationMargin = atmosphericRefraction.getComputationParameters().getInverseLocMargin();
  826.                             throw new RuggedException(RuggedMessages.SENSOR_PIXEL_NOT_FOUND_IN_PIXELS_LINE, sensorPixelGrid[uIndex][vIndex].getPixelNumber(),
  827.                                                       -invLocationMargin, invLocationMargin + sensor.getNbPixels() - 1, invLocationMargin);
  828.                         }
  829.                     }

  831.                 } else { // groundGrid[uIndex][vIndex] == null: impossible to compute inverse loc because ground point not defined

  833.                     sensorPixelGrid[uIndex][vIndex] = null;

  835.                 } // groundGrid[uIndex][vIndex] != null
  836.             } // end loop vIndex
  837.         } // end loop uIndex

  839.         // Reactivate the dump
  840.         DumpManager.resume(wasSuspended);

  842.         // The sensor grid computed WITHOUT atmospheric refraction correction
  843.         return sensorPixelGrid;
  844.     }

  846.     /** Check if pixel is inside the sensor with a margin.
  847.      * @param pixel pixel to check (may be null if not found)
  848.      * @param sensor the line sensor
  849.      * @return true if the pixel is inside the sensor
  850.      */
  851.     private boolean pixelIsInside(final SensorPixel pixel, final LineSensor sensor) {
  852.         // Get the inverse location margin
  853.         final double invLocationMargin = atmosphericRefraction.getComputationParameters().getInverseLocMargin();

  855.         return pixel != null && pixel.getPixelNumber() >= -invLocationMargin && pixel.getPixelNumber() < invLocationMargin + sensor.getNbPixels() - 1;
  856.     }

  858.     /** Computation, for the sensor pixels grid, of the direct location WITH atmospheric refraction.
  859.      * (full computation)
  860.      * @param pixelGrid the pixel grid
  861.      * @param lineGrid the line grid
  862.      * @param sensor the line sensor
  863.      * @return the ground grid computed with atmosphere
  864.      * @since 2.1
  865.      */
  866.     private GeodeticPoint[][] computeDirectLocOnGridWithAtmosphere(final double[] pixelGrid, final double[] lineGrid,
  867.                                                                    final LineSensor sensor) {

  869.         // Deactivate the dump because no need to keep intermediate computations of direct loc (can be regenerate)
  870.         final Boolean wasSuspended = DumpManager.suspend();

  872.         final int nbPixelGrid = pixelGrid.length;
  873.         final int nbLineGrid = lineGrid.length;
  874.         final GeodeticPoint[][] groundGridWithAtmosphere = new GeodeticPoint[nbPixelGrid][nbLineGrid];
  875.         final Vector3D sensorPosition = sensor.getPosition();

  877.         for (int uIndex = 0; uIndex < nbPixelGrid; uIndex++) {
  878.             final double pixelNumber = pixelGrid[uIndex];
  879.             for (int vIndex = 0; vIndex < nbLineGrid; vIndex++) {
  880.                 final double lineNumber = lineGrid[vIndex];
  881.                 final AbsoluteDate date = sensor.getDate(lineNumber);
  882.                 final Vector3D los = sensor.getLOS(date, pixelNumber);
  883.                 try {
  884.                     // Compute the direct location for the current node
  885.                     groundGridWithAtmosphere[uIndex][vIndex] = directLocation(date, sensorPosition, los);

  887.                 } catch (RuggedException re) { // This should never happen
  888.                     // In order for the dump to end nicely
  889.                     DumpManager.endNicely();
  890.                     throw new RuggedInternalError(re);
  891.                 }
  892.             } // end loop vIndex
  893.         } // end loop uIndex

  895.         // Reactivate the dump
  896.         DumpManager.resume(wasSuspended);

  898.         // The ground grid computed WITH atmospheric refraction correction
  899.         return groundGridWithAtmosphere;
  900.     }

  902.     /** Compute distances between two line sensors.
  903.      * @param sensorA line sensor A
  904.      * @param dateA current date for sensor A
  905.      * @param pixelA pixel index for sensor A
  906.      * @param scToBodyA spacecraft to body transform for sensor A
  907.      * @param sensorB line sensor B
  908.      * @param dateB current date for sensor B
  909.      * @param pixelB pixel index for sensor B
  910.      * @return distances computed between LOS and to the ground
  911.      * @since 2.0
  912.      */
  913.     public double[] distanceBetweenLOS(final LineSensor sensorA, final AbsoluteDate dateA, final double pixelA,
  914.                                        final SpacecraftToObservedBody scToBodyA,
  915.                                        final LineSensor sensorB, final AbsoluteDate dateB, final double pixelB) {

  917.         // Compute the approximate transform between spacecraft and observed body
  918.         // from Rugged instance A
  919.         final Transform scToInertA = scToBodyA.getScToInertial(dateA);
  920.         final Transform inertToBodyA = scToBodyA.getInertialToBody(dateA);
  921.         final Transform transformScToBodyA = new Transform(dateA, scToInertA, inertToBodyA);

  923.         // from (current) Rugged instance B
  924.         final Transform scToInertB = scToBody.getScToInertial(dateB);
  925.         final Transform inertToBodyB = scToBody.getInertialToBody(dateB);
  926.         final Transform transformScToBodyB = new Transform(dateB, scToInertB, inertToBodyB);

  928.         // Get sensors LOS into local frame
  929.         final Vector3D vALocal = sensorA.getLOS(dateA, pixelA);
  930.         final Vector3D vBLocal = sensorB.getLOS(dateB, pixelB);

  932.         // Position of sensors into local frame
  933.         final Vector3D sALocal = sensorA.getPosition(); // S_a : sensorA 's position
  934.         final Vector3D sBLocal = sensorB.getPosition(); // S_b : sensorB 's position

  936.         // Get sensors position and LOS into body frame
  937.         final Vector3D sA = transformScToBodyA.transformPosition(sALocal); // S_a : sensorA's position
  938.         final Vector3D vA = transformScToBodyA.transformVector(vALocal);   // V_a : line of sight's vectorA
  939.         final Vector3D sB = transformScToBodyB.transformPosition(sBLocal); // S_b : sensorB's position
  940.         final Vector3D vB = transformScToBodyB.transformVector(vBLocal);   // V_b : line of sight's vectorB

  942.         // Compute distance
  943.         final Vector3D vBase = sB.subtract(sA);            // S_b - S_a
  944.         final double svA = Vector3D.dotProduct(vBase, vA); // SV_a = (S_b - S_a).V_a
  945.         final double svB = Vector3D.dotProduct(vBase, vB); // SV_b = (S_b - S_a).V_b

  947.         final double vAvB = Vector3D.dotProduct(vA, vB); // V_a.V_b

  949.         // Compute lambda_b = (SV_a * V_a.V_b - SV_b) / (1 - (V_a.V_b)²)
  950.         final double lambdaB = (svA * vAvB - svB) / (1 - vAvB * vAvB);

  952.         // Compute lambda_a = SV_a + lambdaB * V_a.V_b
  953.         final double lambdaA = svA + lambdaB * vAvB;

  955.         // Compute vector M_a = S_a + lambda_a * V_a
  956.         final Vector3D mA = sA.add(vA.scalarMultiply(lambdaA));
  957.         // Compute vector M_b = S_b + lambda_b * V_b
  958.         final Vector3D mB = sB.add(vB.scalarMultiply(lambdaB));

  960.         // Compute vector M_a -> M_B for which distance between LOS is minimum
  961.         final Vector3D vDistanceMin = mB.subtract(mA); // M_b - M_a

  963.         // Compute vector from mid point of vector M_a -> M_B to the ground (corresponds to minimum elevation)
  964.         final Vector3D midPoint = (mB.add(mA)).scalarMultiply(0.5);

  966.         // Get the euclidean norms to compute the minimum distances: between LOS and to the ground
  967.         final double[] distances = {vDistanceMin.getNorm(), midPoint.getNorm()};

  969.         return distances;
  970.     }

  972.     /** Compute distances between two line sensors with derivatives.
  973.      * @param <T> derivative type
  974.      * @param sensorA line sensor A
  975.      * @param dateA current date for sensor A
  976.      * @param pixelA pixel index for sensor A
  977.      * @param scToBodyA spacecraftToBody transform for sensor A
  978.      * @param sensorB line sensor B
  979.      * @param dateB current date for sensor B
  980.      * @param pixelB pixel index for sensor B
  981.      * @param generator generator to use for building {@link DerivativeStructure} instances
  982.      * @return distances computed, with derivatives, between LOS and to the ground
  983.      * @see #distanceBetweenLOS(LineSensor, AbsoluteDate, double, SpacecraftToObservedBody, LineSensor, AbsoluteDate, double)
  984.      */
  985.     public <T extends Derivative<T>> T[] distanceBetweenLOSderivatives(
  986.                                  final LineSensor sensorA, final AbsoluteDate dateA, final double pixelA,
  987.                                  final SpacecraftToObservedBody scToBodyA,
  988.                                  final LineSensor sensorB, final AbsoluteDate dateB, final double pixelB,
  989.                                  final DerivativeGenerator<T> generator) {

  991.         // Compute the approximate transforms between spacecraft and observed body
  992.         // from Rugged instance A
  993.         final Transform scToInertA = scToBodyA.getScToInertial(dateA);
  994.         final Transform inertToBodyA = scToBodyA.getInertialToBody(dateA);
  995.         final Transform transformScToBodyA = new Transform(dateA, scToInertA, inertToBodyA);

  997.         // from (current) Rugged instance B
  998.         final Transform scToInertB = scToBody.getScToInertial(dateB);
  999.         final Transform inertToBodyB = scToBody.getInertialToBody(dateB);
  1000.         final Transform transformScToBodyB = new Transform(dateB, scToInertB, inertToBodyB);

  1002.         // Get sensors LOS into local frame
  1003.         final FieldVector3D<T> vALocal = sensorA.getLOSDerivatives(dateA, pixelA, generator);
  1004.         final FieldVector3D<T> vBLocal = sensorB.getLOSDerivatives(dateB, pixelB, generator);

  1006.         // Get sensors LOS into body frame
  1007.         final FieldVector3D<T> vA = transformScToBodyA.transformVector(vALocal); // V_a : line of sight's vectorA
  1008.         final FieldVector3D<T> vB = transformScToBodyB.transformVector(vBLocal); // V_b : line of sight's vectorB

  1010.         // Position of sensors into local frame
  1011.         final Vector3D sAtmp = sensorA.getPosition();
  1012.         final Vector3D sBtmp = sensorB.getPosition();

  1014.         final T scaleFactor = FieldVector3D.dotProduct(vA.normalize(), vA.normalize()); // V_a.V_a=1

  1016.         // Build a vector from the position and a scale factor (equals to 1).
  1017.         // The vector built will be scaleFactor * sAtmp for example.
  1018.         final FieldVector3D<T> sALocal = new FieldVector3D<>(scaleFactor, sAtmp);
  1019.         final FieldVector3D<T> sBLocal = new FieldVector3D<>(scaleFactor, sBtmp);

  1021.         // Get sensors position into body frame
  1022.         final FieldVector3D<T> sA = transformScToBodyA.transformPosition(sALocal); // S_a : sensorA 's position
  1023.         final FieldVector3D<T> sB = transformScToBodyB.transformPosition(sBLocal); // S_b : sensorB 's position

  1025.         // Compute distance
  1026.         final FieldVector3D<T> vBase = sB.subtract(sA);    // S_b - S_a
  1027.         final T svA = FieldVector3D.dotProduct(vBase, vA); // SV_a = (S_b - S_a).V_a
  1028.         final T svB = FieldVector3D.dotProduct(vBase, vB); // SV_b = (S_b - S_a).V_b

  1030.         final T vAvB = FieldVector3D.dotProduct(vA, vB); // V_a.V_b

  1032.         // Compute lambda_b = (SV_a * V_a.V_b - SV_b) / (1 - (V_a.V_b)²)
  1033.         final T lambdaB = (svA.multiply(vAvB).subtract(svB)).divide(vAvB.multiply(vAvB).subtract(1).negate());

  1035.         // Compute lambda_a = SV_a + lambdaB * V_a.V_b
  1036.         final T lambdaA = vAvB.multiply(lambdaB).add(svA);

  1038.         // Compute vector M_a:
  1039.         final FieldVector3D<T> mA = sA.add(vA.scalarMultiply(lambdaA)); // M_a = S_a + lambda_a * V_a
  1040.         // Compute vector M_b
  1041.         final FieldVector3D<T> mB = sB.add(vB.scalarMultiply(lambdaB)); // M_b = S_b + lambda_b * V_b

  1043.         // Compute vector M_a -> M_B for which distance between LOS is minimum
  1044.         final FieldVector3D<T> vDistanceMin = mB.subtract(mA); // M_b - M_a

  1046.         // Compute vector from mid point of vector M_a -> M_B to the ground (corresponds to minimum elevation)
  1047.         final FieldVector3D<T> midPoint = (mB.add(mA)).scalarMultiply(0.5);

  1049.         // Get the euclidean norms to compute the minimum distances:
  1050.         // between LOS
  1051.         final T dMin = vDistanceMin.getNorm();
  1052.         // to the ground
  1053.         final T dCentralBody = midPoint.getNorm();

  1055.         final T[] ret = MathArrays.buildArray(dMin.getField(), 2);
  1056.         ret[0] = dMin;
  1057.         ret[1] = dCentralBody;
  1058.         return ret;

  1060.     }


  1063.     /** Get the mean plane crossing finder for a sensor.
  1064.      * @param sensorName name of the line sensor
  1065.      * @param minLine minimum line number
  1066.      * @param maxLine maximum line number
  1067.      * @return mean plane crossing finder
  1068.      */
  1069.     private SensorMeanPlaneCrossing getPlaneCrossing(final String sensorName,
  1070.                                                      final int minLine, final int maxLine) {

  1072.         final LineSensor sensor = getLineSensor(sensorName);
  1073.         SensorMeanPlaneCrossing planeCrossing = finders.get(sensorName);
  1074.         if (planeCrossing == null ||
  1075.             planeCrossing.getMinLine() != minLine ||
  1076.             planeCrossing.getMaxLine() != maxLine) {

  1078.             // create a new finder for the specified sensor and range
  1079.             planeCrossing = new SensorMeanPlaneCrossing(sensor, scToBody, minLine, maxLine,
  1080.                                                         lightTimeCorrection, aberrationOfLightCorrection,
  1081.                                                         MAX_EVAL, COARSE_INVERSE_LOCATION_ACCURACY);

  1083.             // store the finder, in order to reuse it
  1084.             // (and save some computation done in its constructor)
  1085.             setPlaneCrossing(planeCrossing);

  1087.         }

  1089.         return planeCrossing;
  1090.     }

  1092.     /** Set the mean plane crossing finder for a sensor.
  1093.      * @param planeCrossing plane crossing finder
  1094.      */
  1095.     private void setPlaneCrossing(final SensorMeanPlaneCrossing planeCrossing) {
  1096.         finders.put(planeCrossing.getSensor().getName(), planeCrossing);
  1097.     }

  1099.     /** Inverse location of a point with derivatives.
  1100.      * @param <T> derivative type
  1101.      * @param sensorName name of the line sensor
  1102.      * @param point point to localize
  1103.      * @param minLine minimum line number
  1104.      * @param maxLine maximum line number
  1105.      * @param generator generator to use for building {@link Derivative} instances
  1106.      * @return sensor pixel seeing point with derivatives, or null if point cannot be seen between the
  1107.      * prescribed line numbers
  1108.      * @see #inverseLocation(String, GeodeticPoint, int, int)
  1109.      * @since 2.0
  1110.      */
  1111.     public <T extends Derivative<T>> T[] inverseLocationDerivatives(final String sensorName,
  1112.                                                                     final GeodeticPoint point,
  1113.                                                                     final int minLine,
  1114.                                                                     final int maxLine,
  1115.                                                                     final DerivativeGenerator<T> generator) {

  1117.         final LineSensor sensor = getLineSensor(sensorName);

  1119.         final SensorMeanPlaneCrossing planeCrossing = getPlaneCrossing(sensorName, minLine, maxLine);

  1121.         // find approximately the sensor line at which ground point crosses sensor mean plane
  1122.         final Vector3D   target = ellipsoid.transform(point);
  1123.         final SensorMeanPlaneCrossing.CrossingResult crossingResult = planeCrossing.find(target);
  1124.         if (crossingResult == null) {
  1125.             // target is out of search interval
  1126.             return null;
  1127.         }

  1129.         // find approximately the pixel along this sensor line
  1130.         final SensorPixelCrossing pixelCrossing =
  1131.                 new SensorPixelCrossing(sensor, planeCrossing.getMeanPlaneNormal(),
  1132.                                         crossingResult.getTargetDirection(),
  1133.                                         MAX_EVAL, COARSE_INVERSE_LOCATION_ACCURACY);
  1134.         final double coarsePixel = pixelCrossing.locatePixel(crossingResult.getDate());
  1135.         if (Double.isNaN(coarsePixel)) {
  1136.             // target is out of search interval
  1137.             return null;
  1138.         }

  1140.         // fix line by considering the closest pixel exact position and line-of-sight
  1141.         // (this pixel might point towards a direction slightly above or below the mean sensor plane)
  1142.         final int lowIndex = FastMath.max(0, FastMath.min(sensor.getNbPixels() - 2, (int) FastMath.floor(coarsePixel)));
  1143.         final FieldVector3D<T> lowLOS =
  1144.                         sensor.getLOSDerivatives(crossingResult.getDate(), lowIndex, generator);
  1145.         final FieldVector3D<T> highLOS = sensor.getLOSDerivatives(crossingResult.getDate(), lowIndex + 1, generator);
  1146.         final FieldVector3D<T> localZ = FieldVector3D.crossProduct(lowLOS, highLOS).normalize();
  1147.         final T beta         = FieldVector3D.dotProduct(crossingResult.getTargetDirection(), localZ).acos();
  1148.         final T s            = FieldVector3D.dotProduct(crossingResult.getTargetDirectionDerivative(), localZ);
  1149.         final T minusBetaDer = s.divide(s.multiply(s).subtract(1).negate().sqrt());
  1150.         final T deltaL       = beta.subtract(0.5 * FastMath.PI) .divide(minusBetaDer);
  1151.         final T fixedLine    = deltaL.add(crossingResult.getLine());
  1152.         final FieldVector3D<T> fixedDirection =
  1153.                         new FieldVector3D<>(deltaL.getField().getOne(), crossingResult.getTargetDirection(),
  1154.                                             deltaL, crossingResult.getTargetDirectionDerivative()).normalize();

  1156.         // fix neighbouring pixels
  1157.         final AbsoluteDate fixedDate  = sensor.getDate(fixedLine.getValue());
  1158.         final FieldVector3D<T> fixedX = sensor.getLOSDerivatives(fixedDate, lowIndex, generator);
  1159.         final FieldVector3D<T> fixedZ = FieldVector3D.crossProduct(fixedX, sensor.getLOSDerivatives(fixedDate, lowIndex + 1, generator));
  1160.         final FieldVector3D<T> fixedY = FieldVector3D.crossProduct(fixedZ, fixedX);

  1162.         // fix pixel
  1163.         final T hY         = FieldVector3D.dotProduct(highLOS, fixedY);
  1164.         final T hX         = FieldVector3D.dotProduct(highLOS, fixedX);
  1165.         final T pixelWidth = hY.atan2(hX);
  1166.         final T fY         = FieldVector3D.dotProduct(fixedDirection, fixedY);
  1167.         final T fX         = FieldVector3D.dotProduct(fixedDirection, fixedX);
  1168.         final T alpha      = fY.atan2(fX);
  1169.         final T fixedPixel = alpha.divide(pixelWidth).add(lowIndex);

  1171.         final T[] ret = MathArrays.buildArray(fixedPixel.getField(), 2);
  1172.         ret[0] = fixedLine;
  1173.         ret[1] = fixedPixel;
  1174.         return ret;

  1176.     }

  1178.     /** Get transform from spacecraft to inertial frame.
  1179.      * @param date date of the transform
  1180.      * @return transform from spacecraft to inertial frame
  1181.      */
  1182.     public Transform getScToInertial(final AbsoluteDate date) {
  1183.         return scToBody.getScToInertial(date);
  1184.     }

  1186.     /** Get transform from inertial frame to observed body frame.
  1187.      * @param date date of the transform
  1188.      * @return transform from inertial frame to observed body frame
  1189.      */
  1190.     public Transform getInertialToBody(final AbsoluteDate date) {
  1191.         return scToBody.getInertialToBody(date);
  1192.     }

  1194.     /** Get transform from observed body frame to inertial frame.
  1195.      * @param date date of the transform
  1196.      * @return transform from observed body frame to inertial frame
  1197.      */
  1198.     public Transform getBodyToInertial(final AbsoluteDate date) {
  1199.         return scToBody.getBodyToInertial(date);
  1200.     }

  1202.     /** Get a sensor.
  1203.      * @param sensorName sensor name
  1204.      * @return selected sensor
  1205.      */
  1206.     public LineSensor getLineSensor(final String sensorName) {

  1208.         final LineSensor sensor = sensors.get(sensorName);
  1209.         if (sensor == null) {
  1210.             throw new RuggedException(RuggedMessages.UNKNOWN_SENSOR, sensorName);
  1211.         }
  1212.         return sensor;
  1213.     }

  1215.     /** Get converter between spacecraft and body.
  1216.      * @return the scToBody
  1217.      * @since 2.0
  1218.      */
  1219.     public SpacecraftToObservedBody getScToBody() {
  1220.         return scToBody;
  1221.     }
  1222. }
Created with JaCoCo 0.8.7.202105040129