1   /* Copyright 2013-2025 CS GROUP
2    * Licensed to CS GROUP (CS) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * CS licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *   http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  package org.orekit.rugged.linesensor;
18  
19  import org.hipparchus.analysis.UnivariateFunction;
20  import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
21  import org.hipparchus.analysis.solvers.UnivariateSolver;
22  import org.hipparchus.exception.MathIllegalArgumentException;
23  import org.hipparchus.geometry.euclidean.threed.Vector3D;
24  import org.hipparchus.util.FastMath;
25  import org.orekit.rugged.errors.RuggedException;
26  import org.orekit.rugged.errors.RuggedInternalError;
27  import org.orekit.time.AbsoluteDate;
28  
29  /** Class devoted to locate where ground point crosses a sensor line.
30   * <p>
31   * This class is used in the first stage of inverse location.
32   * </p>
33   * @author Luc Maisonobe
34   */
35  public class SensorPixelCrossing {
36  
37      /** Margin before and after end pixels, in order to avoid search failures near boundaries. */
38      private static final double MARGIN = 10.0;
39  
40      /** Line sensor. */
41      private final LineSensor sensor;
42  
43      /** Cross direction in spacecraft frame. */
44      private final Vector3D cross;
45  
46      /** Maximum number of evaluations. */
47      private final int maxEval;
48  
49      /** Accuracy to use for finding crossing line number. */
50      private final double accuracy;
51  
52      /** Simple constructor.
53       * @param sensor sensor to consider
54       * @param meanNormal mean plane normal of the line sensor
55       * @param targetDirection target direction in spacecraft frame
56       * @param maxEval maximum number of evaluations
57       * @param accuracy accuracy to use for finding crossing line number
58       */
59      public SensorPixelCrossing(final LineSensor sensor, final Vector3D meanNormal,
60                                 final Vector3D targetDirection,
61                                 final int maxEval, final double accuracy) {
62          this.sensor   = sensor;
63          this.cross    = Vector3D.crossProduct(meanNormal, targetDirection).normalize();
64          this.maxEval  = maxEval;
65          this.accuracy = accuracy;
66      }
67  
68      /** Locate pixel along sensor line.
69       * @param date current date
70       * @return pixel location ({@code Double.NaN} if the first and last
71       * pixels of the line do not bracket a location)
72       */
73      public double locatePixel(final AbsoluteDate date) {
74          try {
75  
76              // set up function evaluating to 0.0 where target matches pixel
77              final UnivariateFunction f = new UnivariateFunction() {
78                  /** {@inheritDoc} */
79                  @Override
80                  public double value(final double x) {
81                      try {
82                          return Vector3D.angle(cross, getLOS(date, x)) - 0.5 * FastMath.PI;
83                      } catch (RuggedException re) {
84                          throw new RuggedInternalError(re);
85                      }
86                  }
87              };
88  
89              // find the root
90              final UnivariateSolver solver =
91                      new BracketingNthOrderBrentSolver(0.0, accuracy, 5);
92              return solver.solve(maxEval, f, -MARGIN, sensor.getNbPixels() - 1 + MARGIN);
93  
94          } catch (MathIllegalArgumentException nbe) {
95              // there are no solutions in the search interval
96              return Double.NaN;
97          }
98      }
99  
100     /** Interpolate sensor pixels at some pixel index.
101      * @param date current date
102      * @param x pixel index
103      * @return interpolated direction for specified index
104      */
105     private Vector3D getLOS(final AbsoluteDate date, final double x) {
106 
107         // find surrounding pixels
108         final int iInf = FastMath.max(0, FastMath.min(sensor.getNbPixels() - 2, (int) FastMath.floor(x)));
109         final int iSup = iInf + 1;
110 
111         // interpolate
112         return new Vector3D(iSup - x, sensor.getLOS(date, iInf),
113                             x - iInf, sensor.getLOS(date, iSup)).normalize();
114 
115     }
116 
117 }