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.api;
18
19 import java.util.Collection;
20 import java.util.HashMap;
21 import java.util.Map;
22
23 import org.hipparchus.analysis.differentiation.DerivativeStructure;
24 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
25 import org.hipparchus.geometry.euclidean.threed.Vector3D;
26 import org.hipparchus.util.FastMath;
27 import org.orekit.bodies.GeodeticPoint;
28 import org.orekit.frames.Transform;
29 import org.orekit.rugged.errors.DumpManager;
30 import org.orekit.rugged.errors.RuggedException;
31 import org.orekit.rugged.errors.RuggedInternalError;
32 import org.orekit.rugged.errors.RuggedMessages;
33 import org.orekit.rugged.intersection.IntersectionAlgorithm;
34 import org.orekit.rugged.linesensor.LineSensor;
35 import org.orekit.rugged.linesensor.SensorMeanPlaneCrossing;
36 import org.orekit.rugged.linesensor.SensorPixel;
37 import org.orekit.rugged.linesensor.SensorPixelCrossing;
38 import org.orekit.rugged.refraction.AtmosphericRefraction;
39 import org.orekit.rugged.utils.DSGenerator;
40 import org.orekit.rugged.utils.ExtendedEllipsoid;
41 import org.orekit.rugged.utils.NormalizedGeodeticPoint;
42 import org.orekit.rugged.utils.SpacecraftToObservedBody;
43 import org.orekit.time.AbsoluteDate;
44 import org.orekit.utils.Constants;
45 import org.orekit.utils.PVCoordinates;
46
47 /** Main class of Rugged library API.
48 * @see RuggedBuilder
49 * @author Luc Maisonobe
50 * @author Guylaine Prat
51 * @author Jonathan Guinet
52 * @author Lucie LabatAllee
53 */
54 public class Rugged {
55
56 /** Accuracy to use in the first stage of inverse location.
57 * <p>
58 * This accuracy is only used to locate the point within one
59 * pixel, hence there is no point in choosing a too small value here.
60 * </p>
61 */
62 private static final double COARSE_INVERSE_LOCATION_ACCURACY = 0.01;
63
64 /** Maximum number of evaluations for crossing algorithms. */
65 private static final int MAX_EVAL = 50;
66
67 /** Margin for computation of inverse location with atmospheric refraction correction. */
68 private static final double INVLOC_MARGIN = 0.8;
69
70 /** Threshold for pixel convergence in fixed point method
71 * (for inverse location with atmospheric refraction correction). */
72 private static final double PIXEL_CV_THRESHOLD = 1.e-4;
73
74 /** Threshold for line convergence in fixed point method
75 * (for inverse location with atmospheric refraction correction). */
76 private static final double LINE_CV_THRESHOLD = 1.e-4;
77
78 /** Reference ellipsoid. */
79 private final ExtendedEllipsoid ellipsoid;
80
81 /** Converter between spacecraft and body. */
82 private final SpacecraftToObservedBody scToBody;
83
84 /** Sensors. */
85 private final Map<String, LineSensor> sensors;
86
87 /** Mean plane crossing finders. */
88 private final Map<String, SensorMeanPlaneCrossing> finders;
89
90 /** DEM intersection algorithm. */
91 private final IntersectionAlgorithm algorithm;
92
93 /** Flag for light time correction. */
94 private boolean lightTimeCorrection;
95
96 /** Flag for aberration of light correction. */
97 private boolean aberrationOfLightCorrection;
98
99 /** Rugged name. */
100 private final String name;
101
102 /** Atmospheric refraction for line of sight correction. */
103 private AtmosphericRefraction atmosphericRefraction;
104
105 /** Build a configured instance.
106 * <p>
107 * By default, the instance performs both light time correction (which refers
108 * to ground point motion with respect to inertial frame) and aberration of
109 * light correction (which refers to spacecraft proper velocity). Explicit calls
110 * to {@link #setLightTimeCorrection(boolean) setLightTimeCorrection}
111 * and {@link #setAberrationOfLightCorrection(boolean) setAberrationOfLightCorrection}
112 * can be made after construction if these phenomena should not be corrected.
113 * </p>
114 * @param algorithm algorithm to use for Digital Elevation Model intersection
115 * @param ellipsoid f reference ellipsoid
116 * @param lightTimeCorrection if true, the light travel time between ground
117 * @param aberrationOfLightCorrection if true, the aberration of light
118 * is corrected for more accurate location
119 * and spacecraft is compensated for more accurate location
120 * @param atmosphericRefraction the atmospheric refraction model to be used for more accurate location
121 * @param scToBody transforms interpolator
122 * @param sensors sensors
123 * @param name Rugged name
124 */
125 Rugged(final IntersectionAlgorithm algorithm, final ExtendedEllipsoid ellipsoid, final boolean lightTimeCorrection,
126 final boolean aberrationOfLightCorrection, final AtmosphericRefraction atmosphericRefraction,
127 final SpacecraftToObservedBody scToBody, final Collection<LineSensor> sensors, final String name) {
128
129
130 // space reference
131 this.ellipsoid = ellipsoid;
132
133 // orbit/attitude to body converter
134 this.scToBody = scToBody;
135
136 // intersection algorithm
137 this.algorithm = algorithm;
138
139 // Rugged name
140 // @since 2.0
141 this.name = name;
142
143 this.sensors = new HashMap<String, LineSensor>();
144 for (final LineSensor s : sensors) {
145 this.sensors.put(s.getName(), s);
146 }
147 this.finders = new HashMap<String, SensorMeanPlaneCrossing>();
148
149 this.lightTimeCorrection = lightTimeCorrection;
150 this.aberrationOfLightCorrection = aberrationOfLightCorrection;
151 this.atmosphericRefraction = atmosphericRefraction;
152 }
153
154 /** Get the Rugged name.
155 * @return Rugged name
156 * @since 2.0
157 */
158 public String getName() {
159 return name;
160 }
161
162 /** Get the DEM intersection algorithm.
163 * @return DEM intersection algorithm
164 */
165 public IntersectionAlgorithm getAlgorithm() {
166 return algorithm;
167 }
168
169 /** Get flag for light time correction.
170 * @return true if the light time between ground and spacecraft is
171 * compensated for more accurate location
172 */
173 public boolean isLightTimeCorrected() {
174 return lightTimeCorrection;
175 }
176
177 /** Get flag for aberration of light correction.
178 * @return true if the aberration of light time is corrected
179 * for more accurate location
180 */
181 public boolean isAberrationOfLightCorrected() {
182 return aberrationOfLightCorrection;
183 }
184
185 /** Get the atmospheric refraction model.
186 * @return atmospheric refraction model
187 * @since 2.0
188 */
189 public AtmosphericRefraction getRefractionCorrection() {
190 return atmosphericRefraction;
191 }
192
193 /** Get the line sensors.
194 * @return line sensors
195 */
196 public Collection<LineSensor> getLineSensors() {
197 return sensors.values();
198 }
199
200 /** Get the start of search time span.
201 * @return start of search time span
202 */
203 public AbsoluteDate getMinDate() {
204 return scToBody.getMinDate();
205 }
206
207 /** Get the end of search time span.
208 * @return end of search time span
209 */
210 public AbsoluteDate getMaxDate() {
211 return scToBody.getMaxDate();
212 }
213
214 /** Check if a date is in the supported range.
215 * <p>
216 * The support range is given by the {@code minDate} and
217 * {@code maxDate} construction parameters, with an {@code
218 * overshootTolerance} margin accepted (i.e. a date slightly
219 * before {@code minDate} or slightly after {@code maxDate}
220 * will be considered in range if the overshoot does not exceed
221 * the tolerance set at construction).
222 * </p>
223 * @param date date to check
224 * @return true if date is in the supported range
225 */
226 public boolean isInRange(final AbsoluteDate date) {
227 return scToBody.isInRange(date);
228 }
229
230 /** Get the observed body ellipsoid.
231 * @return observed body ellipsoid
232 */
233 public ExtendedEllipsoid getEllipsoid() {
234 return ellipsoid;
235 }
236
237 /** Direct location of a sensor line.
238 * @param sensorName name of the line sensor
239 * @param lineNumber number of the line to localize on ground
240 * @return ground position of all pixels of the specified sensor line
241 */
242 public GeodeticPoint[] directLocation(final String sensorName, final double lineNumber) {
243
244 final LineSensor sensor = getLineSensor(sensorName);
245 final Vector3D sensorPosition = sensor.getPosition();
246 final AbsoluteDate date = sensor.getDate(lineNumber);
247
248 // Compute the transform for the date
249 // from spacecraft to inertial
250 final Transform scToInert = scToBody.getScToInertial(date);
251 // from inertial to body
252 final Transform inertToBody = scToBody.getInertialToBody(date);
253
254 // Compute spacecraft velocity in inertial frame
255 final Vector3D spacecraftVelocity = scToInert.transformPVCoordinates(PVCoordinates.ZERO).getVelocity();
256 // Compute sensor position in inertial frame
257 // TBN: for simplicity, due to the size of sensor, we consider each pixel to be at sensor position
258 final Vector3D pInert = scToInert.transformPosition(sensorPosition);
259
260 // Compute location of each pixel
261 final GeodeticPoint[] gp = new GeodeticPoint[sensor.getNbPixels()];
262 for (int i = 0; i < sensor.getNbPixels(); ++i) {
263
264 final Vector3D los = sensor.getLOS(date, i);
265 DumpManager.dumpDirectLocation(date, sensorPosition, los, lightTimeCorrection,
266 aberrationOfLightCorrection, atmosphericRefraction != null);
267
268 // compute the line of sight in inertial frame (without correction)
269 final Vector3D obsLInert = scToInert.transformVector(los);
270 final Vector3D lInert;
271
272 if (aberrationOfLightCorrection) {
273 // apply aberration of light correction on LOS
274 lInert = applyAberrationOfLightCorrection(obsLInert, spacecraftVelocity);
275 } else {
276 // don't apply aberration of light correction on LOS
277 lInert = obsLInert;
278 }
279
280 if (lightTimeCorrection) {
281 // compute DEM intersection with light time correction
282 // TBN: for simplicity, due to the size of sensor, we consider each pixel to be at sensor position
283 gp[i] = computeWithLightTimeCorrection(date, sensorPosition, los, scToInert, inertToBody, pInert, lInert);
284
285 } else {
286 // compute DEM intersection without light time correction
287 final Vector3D pBody = inertToBody.transformPosition(pInert);
288 final Vector3D lBody = inertToBody.transformVector(lInert);
289 gp[i] = algorithm.refineIntersection(ellipsoid, pBody, lBody,
290 algorithm.intersection(ellipsoid, pBody, lBody));
291 }
292
293 // compute with atmospheric refraction correction if necessary
294 if (atmosphericRefraction != null && atmosphericRefraction.mustBeComputed()) {
295
296 // apply atmospheric refraction correction
297 final Vector3D pBody = inertToBody.transformPosition(pInert);
298 final Vector3D lBody = inertToBody.transformVector(lInert);
299 gp[i] = atmosphericRefraction.applyCorrection(pBody, lBody, (NormalizedGeodeticPoint) gp[i], algorithm);
300 }
301 DumpManager.dumpDirectLocationResult(gp[i]);
302 }
303 return gp;
304 }
305
306 /** Direct location of a single line-of-sight.
307 * @param date date of the location
308 * @param sensorPosition sensor position in spacecraft frame. For simplicity, due to the size of sensor,
309 * we consider each pixel to be at sensor position
310 * @param los normalized line-of-sight in spacecraft frame
311 * @return ground position of intersection point between specified los and ground
312 */
313 public GeodeticPoint directLocation(final AbsoluteDate date, final Vector3D sensorPosition, final Vector3D los) {
314
315 DumpManager.dumpDirectLocation(date, sensorPosition, los, lightTimeCorrection, aberrationOfLightCorrection,
316 atmosphericRefraction != null);
317
318 // Compute the transforms for the date
319 // from spacecraft to inertial
320 final Transform scToInert = scToBody.getScToInertial(date);
321 // from inertial to body
322 final Transform inertToBody = scToBody.getInertialToBody(date);
323
324 // Compute spacecraft velocity in inertial frame
325 final Vector3D spacecraftVelocity = scToInert.transformPVCoordinates(PVCoordinates.ZERO).getVelocity();
326 // Compute sensor position in inertial frame
327 // TBN: for simplicity, due to the size of sensor, we consider each pixel to be at sensor position
328 final Vector3D pInert = scToInert.transformPosition(sensorPosition);
329
330 // Compute the line of sight in inertial frame (without correction)
331 final Vector3D obsLInert = scToInert.transformVector(los);
332
333 final Vector3D lInert;
334 if (aberrationOfLightCorrection) {
335 // apply aberration of light correction on LOS
336 lInert = applyAberrationOfLightCorrection(obsLInert, spacecraftVelocity);
337 } else {
338 // don't apply aberration of light correction on LOS
339 lInert = obsLInert;
340 }
341
342 // Compute ground location of specified pixel
343 final NormalizedGeodeticPoint gp;
344
345 if (lightTimeCorrection) {
346 // compute DEM intersection with light time correction
347 // TBN: for simplicity, due to the size of sensor, we consider each pixel to be at sensor position
348 gp = computeWithLightTimeCorrection(date, sensorPosition, los, scToInert, inertToBody, pInert, lInert);
349
350 } else {
351 // compute DEM intersection without light time correction
352 final Vector3D pBody = inertToBody.transformPosition(pInert);
353 final Vector3D lBody = inertToBody.transformVector(lInert);
354 gp = algorithm.refineIntersection(ellipsoid, pBody, lBody,
355 algorithm.intersection(ellipsoid, pBody, lBody));
356 }
357
358 NormalizedGeodeticPoint result = gp;
359
360 // compute the ground location with atmospheric correction if asked for
361 if (atmosphericRefraction != null && atmosphericRefraction.mustBeComputed()) {
362
363 // apply atmospheric refraction correction
364 final Vector3D pBody = inertToBody.transformPosition(pInert);
365 final Vector3D lBody = inertToBody.transformVector(lInert);
366 result = atmosphericRefraction.applyCorrection(pBody, lBody, gp, algorithm);
367
368 } // end test on atmosphericRefraction != null
369
370 DumpManager.dumpDirectLocationResult(result);
371 return result;
372 }
373
374 /** Find the date at which sensor sees a ground point.
375 * <p>
376 * This method is a partial {@link #inverseLocation(String, GeodeticPoint, int, int) inverse location} focusing only on date.
377 * </p>
378 * <p>
379 * The point is given only by its latitude and longitude, the elevation is
380 * computed from the Digital Elevation Model.
381 * </p>
382 * <p>
383 * Note that for each sensor name, the {@code minLine} and {@code maxLine} settings
384 * are cached, because they induce costly frames computation. So these settings
385 * should not be tuned very finely and changed at each call, but should rather be
386 * a few thousand lines wide and refreshed only when needed. If for example an
387 * inverse location is roughly estimated to occur near line 53764 (for example
388 * using {@link org.orekit.rugged.utils.RoughVisibilityEstimator}), {@code minLine}
389 * and {@code maxLine} could be set for example to 50000 and 60000, which would
390 * be OK also if next line inverse location is expected to occur near line 53780,
391 * and next one ... The setting could be changed for example to 55000 and 65000 when
392 * an inverse location is expected to occur after 55750. Of course, these values
393 * are only an example and should be adjusted depending on mission needs.
394 * </p>
395 * @param sensorName name of the line sensor
396 * @param latitude ground point latitude (rad)
397 * @param longitude ground point longitude (rad)
398 * @param minLine minimum line number
399 * @param maxLine maximum line number
400 * @return date at which ground point is seen by line sensor
401 * @see #inverseLocation(String, double, double, int, int)
402 * @see org.orekit.rugged.utils.RoughVisibilityEstimator
403 */
404 public AbsoluteDate dateLocation(final String sensorName,
405 final double latitude, final double longitude,
406 final int minLine, final int maxLine) {
407
408 final GeodeticPoint groundPoint =
409 new GeodeticPoint(latitude, longitude, algorithm.getElevation(latitude, longitude));
410 return dateLocation(sensorName, groundPoint, minLine, maxLine);
411 }
412
413 /** Find the date at which sensor sees a ground point.
414 * <p>
415 * This method is a partial {@link #inverseLocation(String,
416 * GeodeticPoint, int, int) inverse location} focusing only on date.
417 * </p>
418 * <p>
419 * Note that for each sensor name, the {@code minLine} and {@code maxLine} settings
420 * are cached, because they induce costly frames computation. So these settings
421 * should not be tuned very finely and changed at each call, but should rather be
422 * a few thousand lines wide and refreshed only when needed. If for example an
423 * inverse location is roughly estimated to occur near line 53764 (for example
424 * using {@link org.orekit.rugged.utils.RoughVisibilityEstimator}), {@code minLine}
425 * and {@code maxLine} could be set for example to 50000 and 60000, which would
426 * be OK also if next line inverse location is expected to occur near line 53780,
427 * and next one ... The setting could be changed for example to 55000 and 65000 when
428 * an inverse location is expected to occur after 55750. Of course, these values
429 * are only an example and should be adjusted depending on mission needs.
430 * </p>
431 * @param sensorName name of the line sensor
432 * @param point point to localize
433 * @param minLine minimum line number
434 * @param maxLine maximum line number
435 * @return date at which ground point is seen by line sensor
436 * @see #inverseLocation(String, GeodeticPoint, int, int)
437 * @see org.orekit.rugged.utils.RoughVisibilityEstimator
438 */
439 public AbsoluteDate dateLocation(final String sensorName, final GeodeticPoint point,
440 final int minLine, final int maxLine) {
441
442 final LineSensor sensor = getLineSensor(sensorName);
443 final SensorMeanPlaneCrossing planeCrossing = getPlaneCrossing(sensorName, minLine, maxLine);
444
445 // find approximately the sensor line at which ground point crosses sensor mean plane
446 final Vector3D target = ellipsoid.transform(point);
447 final SensorMeanPlaneCrossing.CrossingResult crossingResult = planeCrossing.find(target);
448 if (crossingResult == null) {
449 // target is out of search interval
450 return null;
451 } else {
452 return sensor.getDate(crossingResult.getLine());
453 }
454 }
455
456 /** Inverse location of a ground point.
457 * <p>
458 * The point is given only by its latitude and longitude, the elevation is
459 * computed from the Digital Elevation Model.
460 * </p>
461 * <p>
462 * Note that for each sensor name, the {@code minLine} and {@code maxLine} settings
463 * are cached, because they induce costly frames computation. So these settings
464 * should not be tuned very finely and changed at each call, but should rather be
465 * a few thousand lines wide and refreshed only when needed. If for example an
466 * inverse location is roughly estimated to occur near line 53764 (for example
467 * using {@link org.orekit.rugged.utils.RoughVisibilityEstimator}), {@code minLine}
468 * and {@code maxLine} could be set for example to 50000 and 60000, which would
469 * be OK also if next line inverse location is expected to occur near line 53780,
470 * and next one ... The setting could be changed for example to 55000 and 65000 when
471 * an inverse location is expected to occur after 55750. Of course, these values
472 * are only an example and should be adjusted depending on mission needs.
473 * </p>
474 * @param sensorName name of the line sensor
475 * @param latitude ground point latitude (rad)
476 * @param longitude ground point longitude (rad)
477 * @param minLine minimum line number
478 * @param maxLine maximum line number
479 * @return sensor pixel seeing ground point, or null if ground point cannot
480 * be seen between the prescribed line numbers
481 * @see org.orekit.rugged.utils.RoughVisibilityEstimator
482 */
483 public SensorPixel inverseLocation(final String sensorName,
484 final double latitude, final double longitude,
485 final int minLine, final int maxLine) {
486
487 final GeodeticPoint groundPoint = new GeodeticPoint(latitude, longitude, algorithm.getElevation(latitude, longitude));
488 return inverseLocation(sensorName, groundPoint, minLine, maxLine);
489 }
490
491 /** Inverse location of a point.
492 * <p>
493 * Note that for each sensor name, the {@code minLine} and {@code maxLine} settings
494 * are cached, because they induce costly frames computation. So these settings
495 * should not be tuned very finely and changed at each call, but should rather be
496 * a few thousand lines wide and refreshed only when needed. If for example an
497 * inverse location is roughly estimated to occur near line 53764 (for example
498 * using {@link org.orekit.rugged.utils.RoughVisibilityEstimator}), {@code minLine}
499 * and {@code maxLine} could be set for example to 50000 and 60000, which would
500 * be OK also if next line inverse location is expected to occur near line 53780,
501 * and next one ... The setting could be changed for example to 55000 and 65000 when
502 * an inverse location is expected to occur after 55750. Of course, these values
503 * are only an example and should be adjusted depending on mission needs.
504 * </p>
505 * @param sensorName name of the line sensor
506 * @param point geodetic point to localize
507 * @param minLine minimum line number where the search will be performed
508 * @param maxLine maximum line number where the search will be performed
509 * @return sensor pixel seeing point, or null if point cannot be seen between the
510 * prescribed line numbers
511 * @see #dateLocation(String, GeodeticPoint, int, int)
512 * @see org.orekit.rugged.utils.RoughVisibilityEstimator
513 */
514 public SensorPixel inverseLocation(final String sensorName, final GeodeticPoint point,
515 final int minLine, final int maxLine) {
516
517 final LineSensor sensor = getLineSensor(sensorName);
518 DumpManager.dumpInverseLocation(sensor, point, ellipsoid, minLine, maxLine, lightTimeCorrection,
519 aberrationOfLightCorrection, atmosphericRefraction != null);
520
521 final SensorMeanPlaneCrossing planeCrossing = getPlaneCrossing(sensorName, minLine, maxLine);
522 DumpManager.dumpSensorMeanPlane(planeCrossing);
523
524 if (atmosphericRefraction == null || !atmosphericRefraction.mustBeComputed()) {
525 // Compute inverse location WITHOUT atmospheric refraction
526 return findSensorPixelWithoutAtmosphere(point, sensor, planeCrossing);
527 } else {
528 // Compute inverse location WITH atmospheric refraction
529 return findSensorPixelWithAtmosphere(point, sensor, minLine, maxLine);
530 }
531 }
532
533 /** Apply aberration of light correction (for direct location).
534 * @param spacecraftVelocity spacecraft velocity in inertial frame
535 * @param obsLInert line of sight in inertial frame
536 * @return line of sight with aberration of light correction
537 */
538 private Vector3D applyAberrationOfLightCorrection(final Vector3D obsLInert, final Vector3D spacecraftVelocity) {
539
540 // As the spacecraft velocity is small with respect to speed of light,
541 // we use classical velocity addition and not relativistic velocity addition
542 // we look for a positive k such that: c * lInert + vsat = k * obsLInert
543 // with lInert normalized
544 final double a = obsLInert.getNormSq();
545 final double b = -Vector3D.dotProduct(obsLInert, spacecraftVelocity);
546 final double c = spacecraftVelocity.getNormSq() - Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT;
547
548 // a > 0 and c < 0
549 final double s = FastMath.sqrt(b * b - a * c);
550
551 // Only the k > 0 are kept as solutions (the solutions: -(s+b)/a and c/(s-b) are useless)
552 final double k = (b > 0) ? -c / (s + b) : (s - b) / a;
553
554 final Vector3D lInert = new Vector3D( k / Constants.SPEED_OF_LIGHT, obsLInert, -1.0 / Constants.SPEED_OF_LIGHT, spacecraftVelocity);
555 return lInert;
556 }
557
558 /** Compute the DEM intersection with light time correction.
559 * @param date date of the los
560 * @param sensorPosition sensor position in spacecraft frame
561 * @param los los in spacecraft frame
562 * @param scToInert transform for the date from spacecraft to inertial
563 * @param inertToBody transform for the date from inertial to body
564 * @param pInert sensor position in inertial frame
565 * @param lInert line of sight in inertial frame
566 * @return geodetic point with light time correction
567 */
568 private NormalizedGeodeticPoint computeWithLightTimeCorrection(final AbsoluteDate date,
569 final Vector3D sensorPosition, final Vector3D los,
570 final Transform scToInert, final Transform inertToBody,
571 final Vector3D pInert, final Vector3D lInert) {
572
573 // compute the approximate transform between spacecraft and observed body
574 final Transform approximate = new Transform(date, scToInert, inertToBody);
575
576 final Vector3D sL = approximate.transformVector(los);
577 final Vector3D sP = approximate.transformPosition(sensorPosition);
578
579 final Vector3D eP1 = ellipsoid.transform(ellipsoid.pointOnGround(sP, sL, 0.0));
580 final double deltaT1 = eP1.distance(sP) / Constants.SPEED_OF_LIGHT;
581 final Transform shifted1 = inertToBody.shiftedBy(-deltaT1);
582 final NormalizedGeodeticPoint gp1 = algorithm.intersection(ellipsoid,
583 shifted1.transformPosition(pInert),
584 shifted1.transformVector(lInert));
585
586 final Vector3D eP2 = ellipsoid.transform(gp1);
587 final double deltaT2 = eP2.distance(sP) / Constants.SPEED_OF_LIGHT;
588 final Transform shifted2 = inertToBody.shiftedBy(-deltaT2);
589 return algorithm.refineIntersection(ellipsoid,
590 shifted2.transformPosition(pInert),
591 shifted2.transformVector(lInert),
592 gp1);
593 }
594
595 /**
596 * Find the sensor pixel WITHOUT atmospheric refraction correction.
597 * @param point geodetic point to localize
598 * @param sensor the line sensor
599 * @param planeCrossing the sensor mean plane crossing
600 * @return the sensor pixel crossing or null if cannot be found
601 * @since 2.1
602 */
603 private SensorPixel findSensorPixelWithoutAtmosphere(final GeodeticPoint point,
604 final LineSensor sensor, final SensorMeanPlaneCrossing planeCrossing) {
605
606 // find approximately the sensor line at which ground point crosses sensor mean plane
607 final Vector3D target = ellipsoid.transform(point);
608 final SensorMeanPlaneCrossing.CrossingResult crossingResult = planeCrossing.find(target);
609 if (crossingResult == null) {
610 // target is out of search interval
611 return null;
612 }
613
614 // find approximately the pixel along this sensor line
615 final SensorPixelCrossing pixelCrossing =
616 new SensorPixelCrossing(sensor, planeCrossing.getMeanPlaneNormal(),
617 crossingResult.getTargetDirection(),
618 MAX_EVAL, COARSE_INVERSE_LOCATION_ACCURACY);
619 final double coarsePixel = pixelCrossing.locatePixel(crossingResult.getDate());
620 if (Double.isNaN(coarsePixel)) {
621 // target is out of search interval
622 return null;
623 }
624
625 // fix line by considering the closest pixel exact position and line-of-sight
626 // (this pixel might point towards a direction slightly above or below the mean sensor plane)
627 final int lowIndex = FastMath.max(0, FastMath.min(sensor.getNbPixels() - 2, (int) FastMath.floor(coarsePixel)));
628 final Vector3D lowLOS = sensor.getLOS(crossingResult.getDate(), lowIndex);
629 final Vector3D highLOS = sensor.getLOS(crossingResult.getDate(), lowIndex + 1);
630 final Vector3D localZ = Vector3D.crossProduct(lowLOS, highLOS).normalize();
631 final double beta = FastMath.acos(Vector3D.dotProduct(crossingResult.getTargetDirection(), localZ));
632 final double s = Vector3D.dotProduct(crossingResult.getTargetDirectionDerivative(), localZ);
633 final double betaDer = -s / FastMath.sqrt(1 - s * s);
634 final double deltaL = (0.5 * FastMath.PI - beta) / betaDer;
635 final double fixedLine = crossingResult.getLine() + deltaL;
636 final Vector3D fixedDirection = new Vector3D(1, crossingResult.getTargetDirection(),
637 deltaL, crossingResult.getTargetDirectionDerivative()).normalize();
638
639 // fix neighbouring pixels
640 final AbsoluteDate fixedDate = sensor.getDate(fixedLine);
641 final Vector3D fixedX = sensor.getLOS(fixedDate, lowIndex);
642 final Vector3D fixedZ = Vector3D.crossProduct(fixedX, sensor.getLOS(fixedDate, lowIndex + 1));
643 final Vector3D fixedY = Vector3D.crossProduct(fixedZ, fixedX);
644
645 // fix pixel
646 final double pixelWidth = FastMath.atan2(Vector3D.dotProduct(highLOS, fixedY),
647 Vector3D.dotProduct(highLOS, fixedX));
648 final double alpha = FastMath.atan2(Vector3D.dotProduct(fixedDirection, fixedY),
649 Vector3D.dotProduct(fixedDirection, fixedX));
650 final double fixedPixel = lowIndex + alpha / pixelWidth;
651
652 final SensorPixelorPixel.html#SensorPixel">SensorPixel result = new SensorPixel(fixedLine, fixedPixel);
653 DumpManager.dumpInverseLocationResult(result);
654
655 return result;
656 }
657
658 /**
659 * Find the sensor pixel WITH atmospheric refraction correction.
660 * @param point geodetic point to localize
661 * @param sensor the line sensor
662 * @param minLine minimum line number where the search will be performed
663 * @param maxLine maximum line number where the search will be performed
664 * @return the sensor pixel crossing or null if cannot be found
665 * @since 2.1
666 */
667 private SensorPixel findSensorPixelWithAtmosphere(final GeodeticPoint point,
668 final LineSensor sensor, final int minLine, final int maxLine) {
669
670 // TBN: there is no direct way to compute the inverse location.
671 // The method is based on an interpolation grid associated with the fixed point method
672
673 final String sensorName = sensor.getName();
674
675 // Compute a correction grid (at sensor level)
676 // ===========================================
677 // Need to be computed only once for a given sensor (with the same minLine and maxLine)
678 if (atmosphericRefraction.getBifPixel() == null || atmosphericRefraction.getBifLine() == null || // lazy evaluation
679 (!atmosphericRefraction.isSameContext(sensorName, minLine, maxLine))) { // Must be recomputed if the context changed
680
681 // Definition of a regular grid (at sensor level)
682 atmosphericRefraction.configureCorrectionGrid(sensor, minLine, maxLine);
683
684 // Get the grid nodes
685 final int nbPixelGrid = atmosphericRefraction.getComputationParameters().getNbPixelGrid();
686 final int nbLineGrid = atmosphericRefraction.getComputationParameters().getNbLineGrid();
687 final double[] pixelGrid = atmosphericRefraction.getComputationParameters().getUgrid();
688 final double[] lineGrid = atmosphericRefraction.getComputationParameters().getVgrid();
689
690 // Computation, for the sensor grid, of the direct location WITH atmospheric refraction
691 // (full computation)
692 atmosphericRefraction.reactivateComputation();
693 final GeodeticPoint[][] geodeticGridWithAtmosphere = computeDirectLocOnGridWithAtmosphere(pixelGrid, lineGrid, sensor);
694 // pixelGrid and lineGrid are the nodes where the direct loc is computed WITH atmosphere
695
696 // Computation of the inverse location WITHOUT atmospheric refraction for the grid nodes
697 atmosphericRefraction.deactivateComputation();
698 final SensorPixel[][] sensorPixelGridInverseWithout = computeInverseLocOnGridWithoutAtmosphere(geodeticGridWithAtmosphere,
699 nbPixelGrid, nbLineGrid, sensor, minLine, maxLine);
700 atmosphericRefraction.reactivateComputation();
701
702 // Compute the grid correction functions (for pixel and line)
703 atmosphericRefraction.computeGridCorrectionFunctions(sensorPixelGridInverseWithout);
704 }
705
706 // Fixed point method
707 // ==================
708 // Initialization
709 // --------------
710 // Deactivate the dump because no need to keep intermediate computations of inverse loc (can be regenerate)
711 final Boolean wasSuspended = DumpManager.suspend();
712
713 // compute the sensor pixel on the desired ground point WITHOUT atmosphere
714 atmosphericRefraction.deactivateComputation();
715 final SensorPixel sp0 = inverseLocation(sensorName, point, minLine, maxLine);
716 atmosphericRefraction.reactivateComputation();
717 // Reactivate the dump
718 DumpManager.resume(wasSuspended);
719
720 if (sp0 == null) {
721 // In order for the dump to end nicely
722 DumpManager.endNicely();
723 // Impossible to find the point in the given min line and max line (without atmosphere)
724 throw new RuggedException(RuggedMessages.INVALID_RANGE_FOR_LINES, minLine, maxLine, "");
725 }
726
727 // set up the starting point of the fixed point method
728 final double pixel0 = sp0.getPixelNumber();
729 final double line0 = sp0.getLineNumber();
730 // Needed data for the dump
731 sensor.dumpRate(line0);
732
733 // Apply fixed point method until convergence in pixel and line
734 // ------------------------------------------------------------
735 // compute the first (pixel, line) value:
736 // initial sensor pixel value + correction due to atmosphere at this same sensor pixel
737 double corrPixelPrevious = pixel0 + atmosphericRefraction.getBifPixel().value(pixel0, line0);
738 double corrLinePrevious = line0 + atmosphericRefraction.getBifLine().value(pixel0, line0);
739
740 double deltaCorrPixel = Double.POSITIVE_INFINITY;
741 double deltaCorrLine = Double.POSITIVE_INFINITY;
742
743 while (deltaCorrPixel > PIXEL_CV_THRESHOLD && deltaCorrLine > LINE_CV_THRESHOLD) {
744 // Compute the current (pixel, line) value =
745 // initial sensor pixel value + correction due to atmosphere on the previous sensor pixel
746 final double corrPixelCurrent = pixel0 + atmosphericRefraction.getBifPixel().value(corrPixelPrevious, corrLinePrevious);
747 final double corrLineCurrent = line0 + atmosphericRefraction.getBifLine().value(corrPixelPrevious, corrLinePrevious);
748
749 // Compute the delta in pixel and line to check the convergence
750 deltaCorrPixel = FastMath.abs(corrPixelCurrent - corrPixelPrevious);
751 deltaCorrLine = FastMath.abs(corrLineCurrent - corrLinePrevious);
752
753 // Store the (pixel, line) for next loop
754 corrPixelPrevious = corrPixelCurrent;
755 corrLinePrevious = corrLineCurrent;
756 }
757 // The sensor pixel is found !
758 final SensorPixelPixel">SensorPixel sensorPixelWithAtmosphere = new SensorPixel(corrLinePrevious, corrPixelPrevious);
759
760 // Dump the found sensorPixel
761 DumpManager.dumpInverseLocationResult(sensorPixelWithAtmosphere);
762
763 return sensorPixelWithAtmosphere;
764 }
765
766 /** Compute the inverse location WITHOUT atmospheric refraction for the geodetic points
767 * associated to the sensor grid nodes.
768 * @param groundGridWithAtmosphere ground grid found for sensor grid nodes with atmosphere
769 * @param nbPixelGrid size of the pixel grid
770 * @param nbLineGrid size of the line grid
771 * @param sensor the line sensor
772 * @param minLine minimum line number where the search will be performed
773 * @param maxLine maximum line number where the search will be performed
774 * @return the sensor pixel grid computed without atmosphere
775 * @since 2.1
776 */
777 private SensorPixel[][] computeInverseLocOnGridWithoutAtmosphere(final GeodeticPoint[][] groundGridWithAtmosphere,
778 final int nbPixelGrid, final int nbLineGrid,
779 final LineSensor sensor, final int minLine, final int maxLine) {
780
781 // Deactivate the dump because no need to keep intermediate computations of inverse loc (can be regenerate)
782 final Boolean wasSuspended = DumpManager.suspend();
783
784 final SensorPixelSensorPixel">SensorPixel[][] sensorPixelGrid = new SensorPixel[nbPixelGrid][nbLineGrid];
785 final String sensorName = sensor.getName();
786
787 for (int uIndex = 0; uIndex < nbPixelGrid; uIndex++) {
788 for (int vIndex = 0; vIndex < nbLineGrid; vIndex++) {
789
790 // Check if the geodetic point exists
791 if (groundGridWithAtmosphere[uIndex][vIndex] != null) {
792 final GeodeticPoint groundPoint = groundGridWithAtmosphere[uIndex][vIndex];
793 final double currentLat = groundPoint.getLatitude();
794 final double currentLon = groundPoint.getLongitude();
795
796 try {
797 // Compute the inverse location for the current node
798 sensorPixelGrid[uIndex][vIndex] = inverseLocation(sensorName, currentLat, currentLon, minLine, maxLine);
799
800 } catch (RuggedException re) { // This should never happen
801 // In order for the dump to end nicely
802 DumpManager.endNicely();
803 throw new RuggedInternalError(re);
804 }
805
806 // Check if the pixel is inside the sensor (with a margin) OR if the inverse location was impossible (null result)
807 if ((sensorPixelGrid[uIndex][vIndex] != null &&
808 (sensorPixelGrid[uIndex][vIndex].getPixelNumber() < (-INVLOC_MARGIN) ||
809 sensorPixelGrid[uIndex][vIndex].getPixelNumber() > (INVLOC_MARGIN + sensor.getNbPixels() - 1))) ||
810 (sensorPixelGrid[uIndex][vIndex] == null) ) {
811 // In order for the dump to end nicely
812 DumpManager.endNicely();
813 // Impossible to find the point in the given min line
814 throw new RuggedException(RuggedMessages.INVALID_RANGE_FOR_LINES, minLine, maxLine, "");
815 }
816
817 } else { // groundGrid[uIndex][vIndex] == null: impossible to compute inverse loc because ground point not defined
818
819 sensorPixelGrid[uIndex][vIndex] = null;
820
821 } // groundGrid[uIndex][vIndex] != null
822 } // end loop vIndex
823 } // end loop uIndex
824
825 // Reactivate the dump
826 DumpManager.resume(wasSuspended);
827
828 // The sensor grid computed WITHOUT atmospheric refraction correction
829 return sensorPixelGrid;
830 }
831
832 /** Computation, for the sensor pixels grid, of the direct location WITH atmospheric refraction.
833 * (full computation)
834 * @param pixelGrid the pixel grid
835 * @param lineGrid the line grid
836 * @param sensor the line sensor
837 * @return the ground grid computed with atmosphere
838 * @since 2.1
839 */
840 private GeodeticPoint[][] computeDirectLocOnGridWithAtmosphere(final double[] pixelGrid, final double[] lineGrid,
841 final LineSensor sensor) {
842
843 // Deactivate the dump because no need to keep intermediate computations of direct loc (can be regenerate)
844 final Boolean wasSuspended = DumpManager.suspend();
845
846 final int nbPixelGrid = pixelGrid.length;
847 final int nbLineGrid = lineGrid.length;
848 final GeodeticPoint[][] groundGridWithAtmosphere = new GeodeticPoint[nbPixelGrid][nbLineGrid];
849 final Vector3D sensorPosition = sensor.getPosition();
850
851 for (int uIndex = 0; uIndex < nbPixelGrid; uIndex++) {
852 final double pixelNumber = pixelGrid[uIndex];
853 for (int vIndex = 0; vIndex < nbLineGrid; vIndex++) {
854 final double lineNumber = lineGrid[vIndex];
855 final AbsoluteDate date = sensor.getDate(lineNumber);
856 final Vector3D los = sensor.getLOS(date, pixelNumber);
857 try {
858 // Compute the direct location for the current node
859 groundGridWithAtmosphere[uIndex][vIndex] = directLocation(date, sensorPosition, los);
860
861 } catch (RuggedException re) { // This should never happen
862 // In order for the dump to end nicely
863 DumpManager.endNicely();
864 throw new RuggedInternalError(re);
865 }
866 } // end loop vIndex
867 } // end loop uIndex
868
869 // Reactivate the dump
870 DumpManager.resume(wasSuspended);
871
872 // The ground grid computed WITH atmospheric refraction correction
873 return groundGridWithAtmosphere;
874 }
875
876 /** Compute distances between two line sensors.
877 * @param sensorA line sensor A
878 * @param dateA current date for sensor A
879 * @param pixelA pixel index for sensor A
880 * @param scToBodyA spacecraft to body transform for sensor A
881 * @param sensorB line sensor B
882 * @param dateB current date for sensor B
883 * @param pixelB pixel index for sensor B
884 * @return distances computed between LOS and to the ground
885 * @since 2.0
886 */
887 public double[] distanceBetweenLOS(final LineSensor sensorA, final AbsoluteDate dateA, final double pixelA,
888 final SpacecraftToObservedBody scToBodyA,
889 final LineSensor sensorB, final AbsoluteDate dateB, final double pixelB) {
890
891 // Compute the approximate transform between spacecraft and observed body
892 // from Rugged instance A
893 final Transform scToInertA = scToBodyA.getScToInertial(dateA);
894 final Transform inertToBodyA = scToBodyA.getInertialToBody(dateA);
895 final Transform transformScToBodyA = new Transform(dateA, scToInertA, inertToBodyA);
896
897 // from (current) Rugged instance B
898 final Transform scToInertB = scToBody.getScToInertial(dateB);
899 final Transform inertToBodyB = scToBody.getInertialToBody(dateB);
900 final Transform transformScToBodyB = new Transform(dateB, scToInertB, inertToBodyB);
901
902 // Get sensors LOS into local frame
903 final Vector3D vALocal = sensorA.getLOS(dateA, pixelA);
904 final Vector3D vBLocal = sensorB.getLOS(dateB, pixelB);
905
906 // Position of sensors into local frame
907 final Vector3D sALocal = sensorA.getPosition(); // S_a : sensorA 's position
908 final Vector3D sBLocal = sensorB.getPosition(); // S_b : sensorB 's position
909
910 // Get sensors position and LOS into body frame
911 final Vector3D sA = transformScToBodyA.transformPosition(sALocal); // S_a : sensorA's position
912 final Vector3D vA = transformScToBodyA.transformVector(vALocal); // V_a : line of sight's vectorA
913 final Vector3D sB = transformScToBodyB.transformPosition(sBLocal); // S_b : sensorB's position
914 final Vector3D vB = transformScToBodyB.transformVector(vBLocal); // V_b : line of sight's vectorB
915
916 // Compute distance
917 final Vector3D vBase = sB.subtract(sA); // S_b - S_a
918 final double svA = Vector3D.dotProduct(vBase, vA); // SV_a = (S_b - S_a).V_a
919 final double svB = Vector3D.dotProduct(vBase, vB); // SV_b = (S_b - S_a).V_b
920
921 final double vAvB = Vector3D.dotProduct(vA, vB); // V_a.V_b
922
923 // Compute lambda_b = (SV_a * V_a.V_b - SV_b) / (1 - (V_a.V_b)²)
924 final double lambdaB = (svA * vAvB - svB) / (1 - vAvB * vAvB);
925
926 // Compute lambda_a = SV_a + lambdaB * V_a.V_b
927 final double lambdaA = svA + lambdaB * vAvB;
928
929 // Compute vector M_a = S_a + lambda_a * V_a
930 final Vector3D mA = sA.add(vA.scalarMultiply(lambdaA));
931 // Compute vector M_b = S_b + lambda_b * V_b
932 final Vector3D mB = sB.add(vB.scalarMultiply(lambdaB));
933
934 // Compute vector M_a -> M_B for which distance between LOS is minimum
935 final Vector3D vDistanceMin = mB.subtract(mA); // M_b - M_a
936
937 // Compute vector from mid point of vector M_a -> M_B to the ground (corresponds to minimum elevation)
938 final Vector3D midPoint = (mB.add(mA)).scalarMultiply(0.5);
939
940 // Get the euclidean norms to compute the minimum distances: between LOS and to the ground
941 final double[] distances = {vDistanceMin.getNorm(), midPoint.getNorm()};
942
943 return distances;
944 }
945
946 /** Compute distances between two line sensors with derivatives.
947 * @param sensorA line sensor A
948 * @param dateA current date for sensor A
949 * @param pixelA pixel index for sensor A
950 * @param scToBodyA spacecraftToBody transform for sensor A
951 * @param sensorB line sensor B
952 * @param dateB current date for sensor B
953 * @param pixelB pixel index for sensor B
954 * @param generator generator to use for building {@link DerivativeStructure} instances
955 * @return distances computed, with derivatives, between LOS and to the ground
956 * @see #distanceBetweenLOS(LineSensor, AbsoluteDate, double, SpacecraftToObservedBody, LineSensor, AbsoluteDate, double)
957 */
958 public DerivativeStructure[] distanceBetweenLOSderivatives(
959 final LineSensor sensorA, final AbsoluteDate dateA, final double pixelA,
960 final SpacecraftToObservedBody scToBodyA,
961 final LineSensor sensorB, final AbsoluteDate dateB, final double pixelB,
962 final DSGenerator generator) {
963
964 // Compute the approximate transforms between spacecraft and observed body
965 // from Rugged instance A
966 final Transform scToInertA = scToBodyA.getScToInertial(dateA);
967 final Transform inertToBodyA = scToBodyA.getInertialToBody(dateA);
968 final Transform transformScToBodyA = new Transform(dateA, scToInertA, inertToBodyA);
969
970 // from (current) Rugged instance B
971 final Transform scToInertB = scToBody.getScToInertial(dateB);
972 final Transform inertToBodyB = scToBody.getInertialToBody(dateB);
973 final Transform transformScToBodyB = new Transform(dateB, scToInertB, inertToBodyB);
974
975 // Get sensors LOS into local frame
976 final FieldVector3D<DerivativeStructure> vALocal = sensorA.getLOSDerivatives(dateA, pixelA, generator);
977 final FieldVector3D<DerivativeStructure> vBLocal = sensorB.getLOSDerivatives(dateB, pixelB, generator);
978
979 // Get sensors LOS into body frame
980 final FieldVector3D<DerivativeStructure> vA = transformScToBodyA.transformVector(vALocal); // V_a : line of sight's vectorA
981 final FieldVector3D<DerivativeStructure> vB = transformScToBodyB.transformVector(vBLocal); // V_b : line of sight's vectorB
982
983 // Position of sensors into local frame
984 final Vector3D sAtmp = sensorA.getPosition();
985 final Vector3D sBtmp = sensorB.getPosition();
986
987 final DerivativeStructure scaleFactor = FieldVector3D.dotProduct(vA.normalize(), vA.normalize()); // V_a.V_a=1
988
989 // Build a vector from the position and a scale factor (equals to 1).
990 // The vector built will be scaleFactor * sAtmp for example.
991 final FieldVector3D<DerivativeStructure> sALocal = new FieldVector3D<DerivativeStructure>(scaleFactor, sAtmp);
992 final FieldVector3D<DerivativeStructure> sBLocal = new FieldVector3D<DerivativeStructure>(scaleFactor, sBtmp);
993
994 // Get sensors position into body frame
995 final FieldVector3D<DerivativeStructure> sA = transformScToBodyA.transformPosition(sALocal); // S_a : sensorA 's position
996 final FieldVector3D<DerivativeStructure> sB = transformScToBodyB.transformPosition(sBLocal); // S_b : sensorB 's position
997
998 // Compute distance
999 final FieldVector3D<DerivativeStructure> vBase = sB.subtract(sA); // S_b - S_a
1000 final DerivativeStructure svA = FieldVector3D.dotProduct(vBase, vA); // SV_a = (S_b - S_a).V_a
1001 final DerivativeStructure svB = FieldVector3D.dotProduct(vBase, vB); // SV_b = (S_b - S_a).V_b
1002
1003 final DerivativeStructure vAvB = FieldVector3D.dotProduct(vA, vB); // V_a.V_b
1004
1005 // Compute lambda_b = (SV_a * V_a.V_b - SV_b) / (1 - (V_a.V_b)²)
1006 final DerivativeStructure lambdaB = (svA.multiply(vAvB).subtract(svB)).divide(vAvB.multiply(vAvB).subtract(1).negate());
1007
1008 // Compute lambda_a = SV_a + lambdaB * V_a.V_b
1009 final DerivativeStructure lambdaA = vAvB.multiply(lambdaB).add(svA);
1010
1011 // Compute vector M_a:
1012 final FieldVector3D<DerivativeStructure> mA = sA.add(vA.scalarMultiply(lambdaA)); // M_a = S_a + lambda_a * V_a
1013 // Compute vector M_b
1014 final FieldVector3D<DerivativeStructure> mB = sB.add(vB.scalarMultiply(lambdaB)); // M_b = S_b + lambda_b * V_b
1015
1016 // Compute vector M_a -> M_B for which distance between LOS is minimum
1017 final FieldVector3D<DerivativeStructure> vDistanceMin = mB.subtract(mA); // M_b - M_a
1018
1019 // Compute vector from mid point of vector M_a -> M_B to the ground (corresponds to minimum elevation)
1020 final FieldVector3D<DerivativeStructure> midPoint = (mB.add(mA)).scalarMultiply(0.5);
1021
1022 // Get the euclidean norms to compute the minimum distances:
1023 // between LOS
1024 final DerivativeStructure dMin = vDistanceMin.getNorm();
1025 // to the ground
1026 final DerivativeStructure dCentralBody = midPoint.getNorm();
1027
1028 return new DerivativeStructure[] {dMin, dCentralBody};
1029 }
1030
1031
1032 /** Get the mean plane crossing finder for a sensor.
1033 * @param sensorName name of the line sensor
1034 * @param minLine minimum line number
1035 * @param maxLine maximum line number
1036 * @return mean plane crossing finder
1037 */
1038 private SensorMeanPlaneCrossing getPlaneCrossing(final String sensorName,
1039 final int minLine, final int maxLine) {
1040
1041 final LineSensor sensor = getLineSensor(sensorName);
1042 SensorMeanPlaneCrossing planeCrossing = finders.get(sensorName);
1043 if (planeCrossing == null ||
1044 planeCrossing.getMinLine() != minLine ||
1045 planeCrossing.getMaxLine() != maxLine) {
1046
1047 // create a new finder for the specified sensor and range
1048 planeCrossing = new SensorMeanPlaneCrossing(sensor, scToBody, minLine, maxLine,
1049 lightTimeCorrection, aberrationOfLightCorrection,
1050 MAX_EVAL, COARSE_INVERSE_LOCATION_ACCURACY);
1051
1052 // store the finder, in order to reuse it
1053 // (and save some computation done in its constructor)
1054 setPlaneCrossing(planeCrossing);
1055
1056 }
1057
1058 return planeCrossing;
1059 }
1060
1061 /** Set the mean plane crossing finder for a sensor.
1062 * @param planeCrossing plane crossing finder
1063 */
1064 private void setPlaneCrossing(final SensorMeanPlaneCrossing planeCrossing) {
1065 finders.put(planeCrossing.getSensor().getName(), planeCrossing);
1066 }
1067
1068 /** Inverse location of a point with derivatives.
1069 * @param sensorName name of the line sensor
1070 * @param point point to localize
1071 * @param minLine minimum line number
1072 * @param maxLine maximum line number
1073 * @param generator generator to use for building {@link DerivativeStructure} instances
1074 * @return sensor pixel seeing point with derivatives, or null if point cannot be seen between the
1075 * prescribed line numbers
1076 * @see #inverseLocation(String, GeodeticPoint, int, int)
1077 * @since 2.0
1078 */
1079
1080 public DerivativeStructure[] inverseLocationDerivatives(final String sensorName,
1081 final GeodeticPoint point,
1082 final int minLine,
1083 final int maxLine,
1084 final DSGenerator generator) {
1085
1086 final LineSensor sensor = getLineSensor(sensorName);
1087
1088 final SensorMeanPlaneCrossing planeCrossing = getPlaneCrossing(sensorName, minLine, maxLine);
1089
1090 // find approximately the sensor line at which ground point crosses sensor mean plane
1091 final Vector3D target = ellipsoid.transform(point);
1092 final SensorMeanPlaneCrossing.CrossingResult crossingResult = planeCrossing.find(target);
1093 if (crossingResult == null) {
1094 // target is out of search interval
1095 return null;
1096 }
1097
1098 // find approximately the pixel along this sensor line
1099 final SensorPixelCrossing pixelCrossing =
1100 new SensorPixelCrossing(sensor, planeCrossing.getMeanPlaneNormal(),
1101 crossingResult.getTargetDirection(),
1102 MAX_EVAL, COARSE_INVERSE_LOCATION_ACCURACY);
1103 final double coarsePixel = pixelCrossing.locatePixel(crossingResult.getDate());
1104 if (Double.isNaN(coarsePixel)) {
1105 // target is out of search interval
1106 return null;
1107 }
1108
1109 // fix line by considering the closest pixel exact position and line-of-sight
1110 // (this pixel might point towards a direction slightly above or below the mean sensor plane)
1111 final int lowIndex = FastMath.max(0, FastMath.min(sensor.getNbPixels() - 2, (int) FastMath.floor(coarsePixel)));
1112 final FieldVector3D<DerivativeStructure> lowLOS =
1113 sensor.getLOSDerivatives(crossingResult.getDate(), lowIndex, generator);
1114 final FieldVector3D<DerivativeStructure> highLOS = sensor.getLOSDerivatives(crossingResult.getDate(), lowIndex + 1, generator);
1115 final FieldVector3D<DerivativeStructure> localZ = FieldVector3D.crossProduct(lowLOS, highLOS).normalize();
1116 final DerivativeStructure beta = FieldVector3D.dotProduct(crossingResult.getTargetDirection(), localZ).acos();
1117 final DerivativeStructure s = FieldVector3D.dotProduct(crossingResult.getTargetDirectionDerivative(), localZ);
1118 final DerivativeStructure minusBetaDer = s.divide(s.multiply(s).subtract(1).negate().sqrt());
1119 final DerivativeStructure deltaL = beta.subtract(0.5 * FastMath.PI) .divide(minusBetaDer);
1120 final DerivativeStructure fixedLine = deltaL.add(crossingResult.getLine());
1121 final FieldVector3D<DerivativeStructure> fixedDirection =
1122 new FieldVector3D<DerivativeStructure>(deltaL.getField().getOne(), crossingResult.getTargetDirection(),
1123 deltaL, crossingResult.getTargetDirectionDerivative()).normalize();
1124
1125 // fix neighbouring pixels
1126 final AbsoluteDate fixedDate = sensor.getDate(fixedLine.getValue());
1127 final FieldVector3D<DerivativeStructure> fixedX = sensor.getLOSDerivatives(fixedDate, lowIndex, generator);
1128 final FieldVector3D<DerivativeStructure> fixedZ = FieldVector3D.crossProduct(fixedX, sensor.getLOSDerivatives(fixedDate, lowIndex + 1, generator));
1129 final FieldVector3D<DerivativeStructure> fixedY = FieldVector3D.crossProduct(fixedZ, fixedX);
1130
1131 // fix pixel
1132 final DerivativeStructure hY = FieldVector3D.dotProduct(highLOS, fixedY);
1133 final DerivativeStructure hX = FieldVector3D.dotProduct(highLOS, fixedX);
1134 final DerivativeStructure pixelWidth = hY.atan2(hX);
1135 final DerivativeStructure fY = FieldVector3D.dotProduct(fixedDirection, fixedY);
1136 final DerivativeStructure fX = FieldVector3D.dotProduct(fixedDirection, fixedX);
1137 final DerivativeStructure alpha = fY.atan2(fX);
1138 final DerivativeStructure fixedPixel = alpha.divide(pixelWidth).add(lowIndex);
1139
1140 return new DerivativeStructure[] {
1141 fixedLine, fixedPixel
1142 };
1143 }
1144
1145 /** Get transform from spacecraft to inertial frame.
1146 * @param date date of the transform
1147 * @return transform from spacecraft to inertial frame
1148 */
1149 public Transform getScToInertial(final AbsoluteDate date) {
1150 return scToBody.getScToInertial(date);
1151 }
1152
1153 /** Get transform from inertial frame to observed body frame.
1154 * @param date date of the transform
1155 * @return transform from inertial frame to observed body frame
1156 */
1157 public Transform getInertialToBody(final AbsoluteDate date) {
1158 return scToBody.getInertialToBody(date);
1159 }
1160
1161 /** Get transform from observed body frame to inertial frame.
1162 * @param date date of the transform
1163 * @return transform from observed body frame to inertial frame
1164 */
1165 public Transform getBodyToInertial(final AbsoluteDate date) {
1166 return scToBody.getBodyToInertial(date);
1167 }
1168
1169 /** Get a sensor.
1170 * @param sensorName sensor name
1171 * @return selected sensor
1172 */
1173 public LineSensor getLineSensor(final String sensorName) {
1174
1175 final LineSensor sensor = sensors.get(sensorName);
1176 if (sensor == null) {
1177 throw new RuggedException(RuggedMessages.UNKNOWN_SENSOR, sensorName);
1178 }
1179 return sensor;
1180 }
1181
1182 /** Get converter between spacecraft and body.
1183 * @return the scToBody
1184 * @since 2.0
1185 */
1186 public SpacecraftToObservedBody getScToBody() {
1187 return scToBody;
1188 }
1189 }